UDirectory::UDirectory(const std::string & path, const std::string & extensions) { extensions_ = uListToVector(uSplit(extensions, ' ')); path_ = path; iFileName_ = fileNames_.begin(); this->update(); }
void UDir::setPath(const std::string & path, const std::string & extensions) { extensions_ = uListToVector(uSplit(extensions, ' ')); path_ = path; fileNames_.clear(); iFileName_ = fileNames_.begin(); this->update(); }
std::string UFile::getExtension(const std::string &filePath) { std::list<std::string> list = uSplit(filePath, '.'); if(list.size()) { return list.back(); } return ""; }
DBReader::DBReader(const std::string & databasePath, float frameRate, bool odometryIgnored, bool ignoreGoalDelay, bool goalsIgnored) : _paths(uSplit(databasePath, ';')), _frameRate(frameRate), _odometryIgnored(odometryIgnored), _ignoreGoalDelay(ignoreGoalDelay), _goalsIgnored(goalsIgnored), _dbDriver(0), _currentId(_ids.end()), _previousStamp(0), _previousMapID(0) { }
// return in bytes long int UProcessInfo::getMemoryUsage() { long int memoryUsage = -1; #ifdef WIN32 HANDLE hProc = GetCurrentProcess(); PROCESS_MEMORY_COUNTERS info; BOOL okay = GetProcessMemoryInfo(hProc, &info, sizeof(info)); if(okay) { memoryUsage = info.WorkingSetSize; } #elif __APPLE__ rusage u; if(getrusage(RUSAGE_SELF, &u) == 0) { memoryUsage = u.ru_maxrss; } #else std::fstream file("/proc/self/status", std::fstream::in); if(file.is_open()) { std::string bytes; while(std::getline(file, bytes)) { if(bytes.find("VmRSS") != bytes.npos) { std::list<std::string> strs = uSplit(bytes, ' '); if(strs.size()>1) { memoryUsage = atol(uValueAt(strs,1).c_str()) * 1024; } break; } } file.close(); } #endif return memoryUsage; }
// format = {Virtual place, Loop closure, level1, level2, l3, l4...} void BayesFilter::setPredictionLC(const std::string & prediction) { std::list<std::string> strValues = uSplit(prediction, ' '); if(strValues.size() < 2) { UERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str()); } else { std::vector<double> tmpValues(strValues.size()); int i=0; bool valid = true; for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter) { tmpValues[i] = std::atof((*iter).c_str()); //UINFO("%d=%e", i, tmpValues[i]); if(tmpValues[i] < 0.0 || tmpValues[i]>1.0) { valid = false; break; } ++i; } if(!valid) { UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str()); } else { _predictionLC = tmpValues; } } _totalPredictionLCValues = 0.0f; for(unsigned int j=0; j<_predictionLC.size(); ++j) { _totalPredictionLCValues += _predictionLC[j]; } }
void DBReader::mainLoop() { OdometryEvent odom = this->getNextData(); if(odom.data().id()) { std::string goalId; double previousStamp = odom.data().stamp(); if(previousStamp == 0) { odom.data().setStamp(UTimer::now()); } if(!_goalsIgnored && odom.data().userDataRaw().type() == CV_8SC1 && odom.data().userDataRaw().cols >= 7 && // including null str ending odom.data().userDataRaw().rows == 1 && memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) { //GOAL format detected, remove it from the user data and send it as goal event std::string goalStr = (const char *)odom.data().userDataRaw().data; if(!goalStr.empty()) { std::list<std::string> strs = uSplit(goalStr, ':'); if(strs.size() == 2) { goalId = *strs.rbegin(); odom.data().setUserData(cv::Mat()); } } } if(!_odometryIgnored) { if(odom.pose().isNull()) { UWARN("Reading the database: odometry is null! " "Please set \"Ignore odometry = true\" if there is " "no odometry in the database."); } this->post(new OdometryEvent(odom)); } else { this->post(new CameraEvent(odom.data())); } if(!goalId.empty()) { double delay = 0.0; if(!_ignoreGoalDelay && _currentId != _ids.end()) { // get stamp for the next signature to compute the delay // that was used originally for planning int weight; std::string label; double stamp; int mapId; Transform localTransform, pose; _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp); if(previousStamp && stamp && stamp > previousStamp) { delay = stamp - previousStamp; } } if(delay > 0.0) { UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...", goalId.c_str(), delay); } else { UWARN("Goal \"%s\" detected, posting it!", goalId.c_str()); } if(uIsInteger(goalId)) { this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str()))); } else { this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); } if(delay > 0.0) { uSleep(delay*1000); } } } else if(!this->isKilled()) { UINFO("no more images..."); if(_paths.size() > 1) { _paths.pop_front(); UWARN("Loading next database \"%s\"...", _paths.front().c_str()); if(!this->init()) { UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str()); this->kill(); this->post(new CameraEvent()); } } else { this->kill(); this->post(new CameraEvent()); } } }
bool RTABMapApp::exportMesh(const std::string & filePath) { bool success = false; //Assemble the meshes if(UFile::getExtension(filePath).compare("obj") == 0) { pcl::TextureMesh textureMesh; std::vector<cv::Mat> textures; int totalPolygons = 0; pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>); { boost::mutex::scoped_lock lock(meshesMutex_); textureMesh.tex_materials.resize(createdMeshes_.size()); textureMesh.tex_polygons.resize(createdMeshes_.size()); textureMesh.tex_coordinates.resize(createdMeshes_.size()); textures.resize(createdMeshes_.size()); int polygonsStep = 0; int oi = 0; for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!= createdMeshes_.end(); ++iter) { UASSERT(!iter->second.cloud->is_dense); if(!iter->second.texture.empty() && iter->second.cloud->size() && iter->second.polygons.size()) { // OBJ format requires normals pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 20); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals; pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals); // create dense cloud pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>); std::vector<pcl::Vertices> densePolygons; std::map<int, int> newToOldIndices; newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh( *cloudWithNormals, iter->second.polygons, *denseCloud, densePolygons); // polygons UASSERT(densePolygons.size()); unsigned int polygonSize = densePolygons.front().vertices.size(); textureMesh.tex_polygons[oi].resize(densePolygons.size()); textureMesh.tex_coordinates[oi].resize(densePolygons.size() * polygonSize); for(unsigned int j=0; j<densePolygons.size(); ++j) { pcl::Vertices vertices = densePolygons[j]; UASSERT(polygonSize == vertices.vertices.size()); for(unsigned int k=0; k<vertices.vertices.size(); ++k) { //uv std::map<int, int>::iterator jter = newToOldIndices.find(vertices.vertices[k]); textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f( float(jter->second % iter->second.cloud->width) / float(iter->second.cloud->width), // u float(iter->second.cloud->height - jter->second / iter->second.cloud->width) / float(iter->second.cloud->height)); // v vertices.vertices[k] += polygonsStep; } textureMesh.tex_polygons[oi][j] = vertices; } totalPolygons += densePolygons.size(); polygonsStep += denseCloud->size(); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose); if(mergedClouds->size() == 0) { *mergedClouds = *transformedCloud; } else { *mergedClouds += *transformedCloud; } textures[oi] = iter->second.texture; textureMesh.tex_materials[oi].tex_illum = 1; textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first); ++oi; } else { UERROR("Texture not set for mesh %d", iter->first); } } textureMesh.tex_materials.resize(oi); textureMesh.tex_polygons.resize(oi); textures.resize(oi); if(textures.size()) { pcl::toPCLPointCloud2(*mergedClouds, textureMesh.cloud); std::string textureDirectory = uSplit(filePath, '.').front(); UINFO("Saving %d textures to %s.", textures.size(), textureDirectory.c_str()); UDirectory::makeDir(textureDirectory); for(unsigned int i=0;i<textures.size(); ++i) { cv::Mat rawImage = rtabmap::uncompressImage(textures[i]); std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png"; cv::imwrite(texFile, rawImage); UINFO("Saved %s (%d bytes).", texFile.c_str(), rawImage.total()*rawImage.channels()); // relative path textureMesh.tex_materials[i].tex_file = uSplit(UFile::getName(filePath), '.').front()+"/"+textureMesh.tex_materials[i].tex_name+".png"; } UINFO("Saving obj (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), totalPolygons, filePath.c_str()); success = pcl::io::saveOBJFile(filePath, textureMesh) == 0; if(success) { UINFO("Saved obj to %s!", filePath.c_str()); } else { UERROR("Failed saving obj to %s!", filePath.c_str()); } } } } else { pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>); std::vector<pcl::Vertices> mergedPolygons; { boost::mutex::scoped_lock lock(meshesMutex_); for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!= createdMeshes_.end(); ++iter) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>); std::vector<pcl::Vertices> densePolygons; rtabmap::util3d::filterNotUsedVerticesFromMesh( *iter->second.cloud, iter->second.polygons, *denseCloud, densePolygons); pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose); if(mergedClouds->size() == 0) { *mergedClouds = *transformedCloud; mergedPolygons = densePolygons; } else { rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, densePolygons); } } } if(mergedClouds->size() && mergedPolygons.size()) { pcl::PolygonMesh mesh; pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud); mesh.polygons = mergedPolygons; UINFO("Saving ply (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), (int)mergedPolygons.size(), filePath.c_str()); success = pcl::io::savePLYFileBinary(filePath, mesh) == 0; if(success) { UINFO("Saved ply to %s!", filePath.c_str()); } else { UERROR("Failed saving ply to %s!", filePath.c_str()); } } } return success; }
void DBReader::mainLoop() { SensorData data = this->getNextData(); if(data.isValid()) { int goalId = 0; double previousStamp = data.stamp(); data.setStamp(UTimer::now()); if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0) { //GOAL format detected, remove it from the user data and send it as goal event std::string goalStr = uBytes2Str(data.userData()); if(!goalStr.empty()) { std::list<std::string> strs = uSplit(goalStr, ':'); if(strs.size() == 2) { goalId = atoi(strs.rbegin()->c_str()); data.setUserData(std::vector<unsigned char>()); } } } if(!_odometryIgnored) { if(data.pose().isNull()) { UWARN("Reading the database: odometry is null! " "Please set \"Ignore odometry = true\" if there is " "no odometry in the database."); } this->post(new OdometryEvent(data)); } else { this->post(new CameraEvent(data)); } if(goalId > 0) { this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); if(_currentId != _ids.end()) { // get stamp for the next signature to compute the delay // that was used originally for planning int weight; std::string label; double stamp; int mapId; Transform localTransform, pose; std::vector<unsigned char> userData; _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData); if(previousStamp && stamp && stamp > previousStamp) { double delay = stamp - previousStamp; UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...", goalId, delay); uSleep(delay*1000); } else { UWARN("stamps = %d=%f %d=%f ", data.id(), data.stamp(), *_currentId, stamp); } } } } else if(!this->isKilled()) { UINFO("no more images..."); this->kill(); this->post(new CameraEvent()); } }
int main(int argc, char** argv) { ros::init(argc, argv, "data_player"); //ULogger::setType(ULogger::kTypeConsole); //ULogger::setLevel(ULogger::kDebug); //ULogger::setEventLevel(ULogger::kWarning); bool publishClock = false; for(int i=1;i<argc;++i) { if(strcmp(argv[i], "--clock") == 0) { publishClock = true; } } ros::NodeHandle nh; ros::NodeHandle pnh("~"); std::string frameId = "base_link"; std::string odomFrameId = "odom"; std::string cameraFrameId = "camera_optical_link"; std::string scanFrameId = "base_laser_link"; double rate = 1.0f; std::string databasePath = ""; bool publishTf = true; int startId = 0; bool useDbStamps = true; pnh.param("frame_id", frameId, frameId); pnh.param("odom_frame_id", odomFrameId, odomFrameId); pnh.param("camera_frame_id", cameraFrameId, cameraFrameId); pnh.param("scan_frame_id", scanFrameId, scanFrameId); pnh.param("rate", rate, rate); // Ratio of the database stamps pnh.param("database", databasePath, databasePath); pnh.param("publish_tf", publishTf, publishTf); pnh.param("start_id", startId, startId); // A general 360 lidar with 0.5 deg increment double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax; pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI); pnh.param<double>("scan_angle_max", scanAngleMax, M_PI); pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0); pnh.param<double>("scan_range_min", scanRangeMin, 0.0); pnh.param<double>("scan_range_max", scanRangeMax, 60); ROS_INFO("frame_id = %s", frameId.c_str()); ROS_INFO("odom_frame_id = %s", odomFrameId.c_str()); ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str()); ROS_INFO("scan_frame_id = %s", scanFrameId.c_str()); ROS_INFO("rate = %f", rate); ROS_INFO("publish_tf = %s", publishTf?"true":"false"); ROS_INFO("start_id = %d", startId); ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false"); if(databasePath.empty()) { ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database)."); return -1; } databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); if(databasePath.size() && databasePath.at(0) != '/') { databasePath = UDirectory::currentDir(true) + databasePath; } ROS_INFO("database = %s", databasePath.c_str()); rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId); if(!reader.init()) { ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str()); return -1; } ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback); ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback); image_transport::ImageTransport it(nh); image_transport::Publisher imagePub; image_transport::Publisher rgbPub; image_transport::Publisher depthPub; image_transport::Publisher leftPub; image_transport::Publisher rightPub; ros::Publisher rgbCamInfoPub; ros::Publisher depthCamInfoPub; ros::Publisher leftCamInfoPub; ros::Publisher rightCamInfoPub; ros::Publisher odometryPub; ros::Publisher scanPub; ros::Publisher scanCloudPub; ros::Publisher globalPosePub; ros::Publisher gpsFixPub; ros::Publisher clockPub; tf2_ros::TransformBroadcaster tfBroadcaster; if(publishClock) { clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1); } UTimer timer; rtabmap::CameraInfo cameraInfo; rtabmap::SensorData data = reader.takeImage(&cameraInfo); rtabmap::OdometryInfo odomInfo; odomInfo.reg.covariance = cameraInfo.odomCovariance; rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); double acquisitionTime = timer.ticks(); while(ros::ok() && odom.data().id()) { ROS_INFO("Reading sensor data %d...", odom.data().id()); ros::Time time(odom.data().stamp()); if(publishClock) { rosgraph_msgs::Clock msg; msg.clock = time; clockPub.publish(msg); } sensor_msgs::CameraInfo camInfoA; //rgb or left sensor_msgs::CameraInfo camInfoB; //depth or right camInfoA.K.assign(0); camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1; camInfoA.R.assign(0); camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1; camInfoA.P.assign(0); camInfoA.P[10] = 1; camInfoA.header.frame_id = cameraFrameId; camInfoA.header.stamp = time; camInfoB = camInfoA; int type = -1; if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) { if(odom.data().cameraModels().size() > 1) { ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet..."); } else { //depth if(odom.data().cameraModels().size()) { camInfoA.D.resize(5,0); camInfoA.P[0] = odom.data().cameraModels()[0].fx(); camInfoA.K[0] = odom.data().cameraModels()[0].fx(); camInfoA.P[5] = odom.data().cameraModels()[0].fy(); camInfoA.K[4] = odom.data().cameraModels()[0].fy(); camInfoA.P[2] = odom.data().cameraModels()[0].cx(); camInfoA.K[2] = odom.data().cameraModels()[0].cx(); camInfoA.P[6] = odom.data().cameraModels()[0].cy(); camInfoA.K[5] = odom.data().cameraModels()[0].cy(); camInfoB = camInfoA; } type=0; if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1); if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1); if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1); if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1); } } else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) { //stereo if(odom.data().stereoCameraModel().isValidForProjection()) { camInfoA.D.resize(8,0); camInfoA.P[0] = odom.data().stereoCameraModel().left().fx(); camInfoA.K[0] = odom.data().stereoCameraModel().left().fx(); camInfoA.P[5] = odom.data().stereoCameraModel().left().fy(); camInfoA.K[4] = odom.data().stereoCameraModel().left().fy(); camInfoA.P[2] = odom.data().stereoCameraModel().left().cx(); camInfoA.K[2] = odom.data().stereoCameraModel().left().cx(); camInfoA.P[6] = odom.data().stereoCameraModel().left().cy(); camInfoA.K[5] = odom.data().stereoCameraModel().left().cy(); camInfoB = camInfoA; camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx } type=1; if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1); if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1); if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1); if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1); } else { if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1); } camInfoA.height = odom.data().imageRaw().rows; camInfoA.width = odom.data().imageRaw().cols; camInfoB.height = odom.data().depthOrRightRaw().rows; camInfoB.width = odom.data().depthOrRightRaw().cols; if(!odom.data().laserScanRaw().isEmpty()) { if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d()) { scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1); if(odom.data().laserScanRaw().angleIncrement() > 0.0f) { ROS_INFO("Scan will be published."); } else { ROS_INFO("Scan will be published with those parameters:"); ROS_INFO(" scan_angle_min=%f", scanAngleMin); ROS_INFO(" scan_angle_max=%f", scanAngleMax); ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement); ROS_INFO(" scan_range_min=%f", scanRangeMin); ROS_INFO(" scan_range_max=%f", scanRangeMax); } } else if(scanCloudPub.getTopic().empty()) { scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1); ROS_INFO("Scan cloud will be published."); } } if(!odom.data().globalPose().isNull() && odom.data().globalPoseCovariance().cols==6 && odom.data().globalPoseCovariance().rows==6) { if(globalPosePub.getTopic().empty()) { globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1); ROS_INFO("Global pose will be published."); } } if(odom.data().gps().stamp() > 0.0) { if(gpsFixPub.getTopic().empty()) { gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1); ROS_INFO("GPS will be published."); } } // publish transforms first if(publishTf) { rtabmap::Transform localTransform; if(odom.data().cameraModels().size() == 1) { localTransform = odom.data().cameraModels()[0].localTransform(); } else if(odom.data().stereoCameraModel().isValidForProjection()) { localTransform = odom.data().stereoCameraModel().left().localTransform(); } if(!localTransform.isNull()) { geometry_msgs::TransformStamped baseToCamera; baseToCamera.child_frame_id = cameraFrameId; baseToCamera.header.frame_id = frameId; baseToCamera.header.stamp = time; rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform); tfBroadcaster.sendTransform(baseToCamera); } if(!odom.pose().isNull()) { geometry_msgs::TransformStamped odomToBase; odomToBase.child_frame_id = frameId; odomToBase.header.frame_id = odomFrameId; odomToBase.header.stamp = time; rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform); tfBroadcaster.sendTransform(odomToBase); } if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty()) { geometry_msgs::TransformStamped baseToLaserScan; baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.header.frame_id = frameId; baseToLaserScan.header.stamp = time; rtabmap_ros::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); tfBroadcaster.sendTransform(baseToLaserScan); } } if(!odom.pose().isNull()) { if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1); if(odometryPub.getNumSubscribers()) { nav_msgs::Odometry odomMsg; odomMsg.child_frame_id = frameId; odomMsg.header.frame_id = odomFrameId; odomMsg.header.stamp = time; rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); UASSERT(odomMsg.pose.covariance.size() == 36 && odom.covariance().total() == 36 && odom.covariance().type() == CV_64FC1); memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); odometryPub.publish(odomMsg); } } // Publish async topics first (so that they can catched by rtabmap before the image topics) if(globalPosePub.getNumSubscribers() > 0 && !odom.data().globalPose().isNull() && odom.data().globalPoseCovariance().cols==6 && odom.data().globalPoseCovariance().rows==6) { geometry_msgs::PoseWithCovarianceStamped msg; rtabmap_ros::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose); memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double)); msg.header.frame_id = frameId; msg.header.stamp = time; globalPosePub.publish(msg); } if(odom.data().gps().stamp() > 0.0) { sensor_msgs::NavSatFix msg; msg.longitude = odom.data().gps().longitude(); msg.latitude = odom.data().gps().latitude(); msg.altitude = odom.data().gps().altitude(); msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error(); msg.header.frame_id = frameId; msg.header.stamp.fromSec(odom.data().gps().stamp()); gpsFixPub.publish(msg); } if(type >= 0) { if(rgbCamInfoPub.getNumSubscribers() && type == 0) { rgbCamInfoPub.publish(camInfoA); } if(leftCamInfoPub.getNumSubscribers() && type == 1) { leftCamInfoPub.publish(camInfoA); } if(depthCamInfoPub.getNumSubscribers() && type == 0) { depthCamInfoPub.publish(camInfoB); } if(rightCamInfoPub.getNumSubscribers() && type == 1) { rightCamInfoPub.publish(camInfoB); } } if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers()) { cv_bridge::CvImage img; if(odom.data().imageRaw().channels() == 1) { img.encoding = sensor_msgs::image_encodings::MONO8; } else { img.encoding = sensor_msgs::image_encodings::BGR8; } img.image = odom.data().imageRaw(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.stamp = time; if(imagePub.getNumSubscribers()) { imagePub.publish(imageRosMsg); } if(rgbPub.getNumSubscribers() && type == 0) { rgbPub.publish(imageRosMsg); } if(leftPub.getNumSubscribers() && type == 1) { leftPub.publish(imageRosMsg); leftCamInfoPub.publish(camInfoA); } } if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0) { cv_bridge::CvImage img; if(odom.data().depthRaw().type() == CV_32FC1) { img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; } else { img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; } img.image = odom.data().depthRaw(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.stamp = time; depthPub.publish(imageRosMsg); depthCamInfoPub.publish(camInfoB); } if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1) { cv_bridge::CvImage img; img.encoding = sensor_msgs::image_encodings::MONO8; img.image = odom.data().rightRaw(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.stamp = time; rightPub.publish(imageRosMsg); rightCamInfoPub.publish(camInfoB); } if(!odom.data().laserScanRaw().isEmpty()) { if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d()) { //inspired from pointcloud_to_laserscan package sensor_msgs::LaserScan msg; msg.header.frame_id = scanFrameId; msg.header.stamp = time; msg.angle_min = scanAngleMin; msg.angle_max = scanAngleMax; msg.angle_increment = scanAngleIncrement; msg.time_increment = 0.0; msg.scan_time = 0; msg.range_min = scanRangeMin; msg.range_max = scanRangeMax; if(odom.data().laserScanRaw().angleIncrement() > 0.0f) { msg.angle_min = odom.data().laserScanRaw().angleMin(); msg.angle_max = odom.data().laserScanRaw().angleMax(); msg.angle_increment = odom.data().laserScanRaw().angleIncrement(); msg.range_min = odom.data().laserScanRaw().rangeMin(); msg.range_max = odom.data().laserScanRaw().rangeMax(); } uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment); msg.ranges.assign(rangesSize, 0.0); const cv::Mat & scan = odom.data().laserScanRaw().data(); for (int i=0; i<scan.cols; ++i) { const float * ptr = scan.ptr<float>(0,i); double range = hypot(ptr[0], ptr[1]); if (range >= msg.range_min && range <=msg.range_max) { double angle = atan2(ptr[1], ptr[0]); if (angle >= msg.angle_min && angle <= msg.angle_max) { int index = (angle - msg.angle_min) / msg.angle_increment; if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0)) { msg.ranges[index] = range; } } } } scanPub.publish(msg); } else if(scanCloudPub.getNumSubscribers()) { sensor_msgs::PointCloud2 msg; pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg); msg.header.frame_id = scanFrameId; msg.header.stamp = time; scanCloudPub.publish(msg); } } if(odom.data().userDataRaw().type() == CV_8SC1 && odom.data().userDataRaw().cols >= 7 && // including null str ending odom.data().userDataRaw().rows == 1 && memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) { //GOAL format detected, remove it from the user data and send it as goal event std::string goalStr = (const char *)odom.data().userDataRaw().data; if(!goalStr.empty()) { std::list<std::string> strs = uSplit(goalStr, ':'); if(strs.size() == 2) { int goalId = atoi(strs.rbegin()->c_str()); if(goalId > 0) { ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId); rtabmap_ros::SetGoal setGoalSrv; setGoalSrv.request.node_id = goalId; setGoalSrv.request.node_label = ""; if(!ros::service::call("set_goal", setGoalSrv)) { ROS_ERROR("Can't call \"set_goal\" service"); } } } } } ros::spinOnce(); while(ros::ok() && paused) { uSleep(100); ros::spinOnce(); } timer.restart(); cameraInfo = rtabmap::CameraInfo(); data = reader.takeImage(&cameraInfo); odomInfo.reg.covariance = cameraInfo.odomCovariance; odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo); acquisitionTime = timer.ticks(); } return 0; }
OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) : odometry_(0), frameId_("base_link"), odomFrameId_("odom"), groundTruthFrameId_(""), publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms paused_(false) { ros::NodeHandle nh; odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1); odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1); odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1); odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1); ros::NodeHandle pnh("~"); Transform initialPose = Transform::getIdentity(); std::string initialPoseStr; std::string tfPrefix; std::string configPath; pnh.param("frame_id", frameId_, frameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); pnh.param("publish_tf", publishTf_, publishTf_); pnh.param("tf_prefix", tfPrefix, tfPrefix); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw" pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("config_path", configPath, configPath); configPath = uReplaceChar(configPath, '~', UDirectory::homeDir()); if(configPath.size() && configPath.at(0) != '/') { configPath = UDirectory::currentDir(true) + configPath; } if(!tfPrefix.empty()) { if(!frameId_.empty()) { frameId_ = tfPrefix + "/" + frameId_; } if(!odomFrameId_.empty()) { odomFrameId_ = tfPrefix + "/" + odomFrameId_; } if(!groundTruthFrameId_.empty()) { groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_; } } if(initialPoseStr.size()) { std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' ')); if(values.size() == 6) { initialPose = Transform( uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]), uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5])); } else { ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). " "Identity will be used...", initialPoseStr.c_str()); } } //parameters parameters_ = Parameters::getDefaultOdometryParameters(stereo); if(!configPath.empty()) { if(UFile::exists(configPath.c_str())) { ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str()); rtabmap::ParametersMap allParameters; Parameters::readINI(configPath.c_str(), allParameters); // only update odometry parameters for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) { ParametersMap::iterator jter = allParameters.find(iter->first); if(jter!=allParameters.end()) { iter->second = jter->second; } } } else { ROS_ERROR("Config file \"%s\" not found!", configPath.c_str()); } } for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) { std::string vStr; bool vBool; int vInt; double vDouble; if(pnh.getParam(iter->first, vStr)) { ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); iter->second = vStr; } else if(pnh.getParam(iter->first, vBool)) { ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); iter->second = uBool2Str(vBool); } else if(pnh.getParam(iter->first, vDouble)) { ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); iter->second = uNumber2Str(vDouble); } else if(pnh.getParam(iter->first, vInt)) { ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); iter->second = uNumber2Str(vInt); } if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8) { ROS_WARN("Parameter min_inliers must be >= 8, setting to 8..."); iter->second = uNumber2Str(8); } } rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv); for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) { rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first); if(jter!=parameters_.end()) { ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str()); jter->second = iter->second; } } // Backward compatibility for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin(); iter!=Parameters::getRemovedParameters().end(); ++iter) { std::string vStr; if(pnh.getParam(iter->first, vStr)) { if(iter->second.first) { // can be migrated parameters_.at(iter->second.second)= vStr; ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } else { if(iter->second.second.empty()) { ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!", iter->first.c_str()); } else { ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", iter->first.c_str(), iter->second.second.c_str()); } } } } int odomStrategy = 0; // BOW Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy); odometry_ = Odometry::create(parameters_); if(!initialPose.isIdentity()) { odometry_->reset(initialPose); } resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this); resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this); pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this); resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this); setLogDebugSrv_ = pnh.advertiseService("log_debug", &OdometryROS::setLogDebug, this); setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this); setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this); setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this); }
void RtabmapThread::handleEvent(UEvent* event) { if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0) { UDEBUG("CameraEvent"); CameraEvent * e = (CameraEvent*)event; if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth) { this->addData(e->data()); } } else if(event->getClassName().compare("OdometryEvent") == 0) { UDEBUG("OdometryEvent"); OdometryEvent * e = (OdometryEvent*)event; if(e->isValid()) { this->addData(e->data()); } } else if(event->getClassName().compare("RtabmapEventCmd") == 0) { RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event; RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd(); if(cmd == RtabmapEventCmd::kCmdInit) { ULOGGER_DEBUG("CMD_INIT"); ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters(); UASSERT(!rtabmapEvent->getStr().empty()); UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second); pushNewState(kStateInit, parameters); } else if(cmd == RtabmapEventCmd::kCmdClose) { ULOGGER_DEBUG("CMD_CLOSE"); pushNewState(kStateClose); } else if(cmd == RtabmapEventCmd::kCmdResetMemory) { ULOGGER_DEBUG("CMD_RESET_MEMORY"); pushNewState(kStateReseting); } else if(cmd == RtabmapEventCmd::kCmdDumpMemory) { ULOGGER_DEBUG("CMD_DUMP_MEMORY"); pushNewState(kStateDumpingMemory); } else if(cmd == RtabmapEventCmd::kCmdDumpPrediction) { ULOGGER_DEBUG("CMD_DUMP_PREDICTION"); pushNewState(kStateDumpingPrediction); } else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph) { UASSERT(!rtabmapEvent->getStr().empty()); ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH"); ParametersMap param; param.insert(ParametersPair("path", rtabmapEvent->getStr())); pushNewState(kStateGeneratingDOTGraph, param); } else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph) { std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';'); UASSERT(values.size() == 3); ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH"); ParametersMap param; param.insert(ParametersPair("path", *values.begin())); param.insert(ParametersPair("id", *(++values.begin()))); param.insert(ParametersPair("margin", *values.rbegin())); pushNewState(kStateGeneratingDOTLocalGraph, param); } else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal) { UASSERT(!rtabmapEvent->getStr().empty()); ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL"); ParametersMap param; param.insert(ParametersPair("path", rtabmapEvent->getStr())); param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStateGeneratingTOROGraphLocal, param); } else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal) { UASSERT(!rtabmapEvent->getStr().empty()); ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL"); ParametersMap param; param.insert(ParametersPair("path", rtabmapEvent->getStr())); param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStateGeneratingTOROGraphGlobal, param); } else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer) { ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER"); pushNewState(kStateCleanDataBuffer); } else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal) { ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL"); ParametersMap param; param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStatePublishingMapLocal, param); } else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal) { ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL"); ParametersMap param; param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStatePublishingMapGlobal, param); } else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal) { ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL"); ParametersMap param; param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStatePublishingTOROGraphLocal, param); } else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal) { ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL"); ParametersMap param; param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt()))); pushNewState(kStatePublishingTOROGraphGlobal, param); } else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap) { ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP"); pushNewState(kStateTriggeringMap); } else if(cmd == RtabmapEventCmd::kCmdPause) { ULOGGER_DEBUG("CMD_PAUSE"); _paused = !_paused; } else { UWARN("Cmd %d unknown!", cmd); } } else if(event->getClassName().compare("ParamEvent") == 0) { ULOGGER_DEBUG("changing parameters"); pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters()); } }
int main(int argc, char** argv) { ros::init(argc, argv, "data_player"); //ULogger::setType(ULogger::kTypeConsole); //ULogger::setLevel(ULogger::kDebug); //ULogger::setEventLevel(ULogger::kWarning); ros::NodeHandle nh; ros::NodeHandle pnh("~"); std::string frameId = "base_link"; std::string odomFrameId = "odom"; std::string cameraFrameId = "camera_optical_link"; std::string scanFrameId = "base_laser_link"; double rate = -1.0f; std::string databasePath = ""; bool publishTf = true; int startId = 0; pnh.param("frame_id", frameId, frameId); pnh.param("odom_frame_id", odomFrameId, odomFrameId); pnh.param("camera_frame_id", cameraFrameId, cameraFrameId); pnh.param("scan_frame_id", scanFrameId, scanFrameId); pnh.param("rate", rate, rate); // Set -1 to use database stamps pnh.param("database", databasePath, databasePath); pnh.param("publish_tf", publishTf, publishTf); pnh.param("start_id", startId, startId); // based on URG-04LX double scanHeight, scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax; pnh.param<double>("scan_height", scanHeight, 0.3); pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI / 2.0); pnh.param<double>("scan_angle_max", scanAngleMax, M_PI / 2.0); pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 360.0); pnh.param<double>("scan_time", scanTime, 1.0 / 10.0); pnh.param<double>("scan_range_min", scanRangeMin, 0.02); pnh.param<double>("scan_range_max", scanRangeMax, 6.0); ROS_INFO("frame_id = %s", frameId.c_str()); ROS_INFO("odom_frame_id = %s", odomFrameId.c_str()); ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str()); ROS_INFO("scan_frame_id = %s", scanFrameId.c_str()); ROS_INFO("rate = %f", rate); ROS_INFO("publish_tf = %s", publishTf?"true":"false"); ROS_INFO("start_id = %d", startId); if(databasePath.empty()) { ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database)."); return -1; } databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); if(databasePath.size() && databasePath.at(0) != '/') { databasePath = UDirectory::currentDir(true) + databasePath; } ROS_INFO("database = %s", databasePath.c_str()); rtabmap::DBReader reader(databasePath, rate, false, false, false, startId); if(!reader.init()) { ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str()); return -1; } ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback); ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback); image_transport::ImageTransport it(nh); image_transport::Publisher imagePub; image_transport::Publisher rgbPub; image_transport::Publisher depthPub; image_transport::Publisher leftPub; image_transport::Publisher rightPub; ros::Publisher rgbCamInfoPub; ros::Publisher depthCamInfoPub; ros::Publisher leftCamInfoPub; ros::Publisher rightCamInfoPub; ros::Publisher odometryPub; ros::Publisher scanPub; tf2_ros::TransformBroadcaster tfBroadcaster; UTimer timer; rtabmap::CameraInfo info; rtabmap::SensorData data = reader.takeImage(&info); rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance); double acquisitionTime = timer.ticks(); while(ros::ok() && odom.data().id()) { ROS_INFO("Reading sensor data %d...", odom.data().id()); ros::Time time = ros::Time::now(); sensor_msgs::CameraInfo camInfoA; //rgb or left sensor_msgs::CameraInfo camInfoB; //depth or right camInfoA.K.assign(0); camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1; camInfoA.R.assign(0); camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1; camInfoA.P.assign(0); camInfoA.P[10] = 1; camInfoA.header.frame_id = cameraFrameId; camInfoA.header.stamp = time; camInfoB = camInfoA; int type = -1; if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) { if(odom.data().cameraModels().size() > 1) { ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet..."); } else { //depth if(odom.data().cameraModels().size()) { camInfoA.D.resize(5,0); camInfoA.P[0] = odom.data().cameraModels()[0].fx(); camInfoA.K[0] = odom.data().cameraModels()[0].fx(); camInfoA.P[5] = odom.data().cameraModels()[0].fy(); camInfoA.K[4] = odom.data().cameraModels()[0].fy(); camInfoA.P[2] = odom.data().cameraModels()[0].cx(); camInfoA.K[2] = odom.data().cameraModels()[0].cx(); camInfoA.P[6] = odom.data().cameraModels()[0].cy(); camInfoA.K[5] = odom.data().cameraModels()[0].cy(); camInfoB = camInfoA; } type=0; if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1); if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1); if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1); if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1); } } else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) { //stereo if(odom.data().stereoCameraModel().isValidForProjection()) { camInfoA.D.resize(8,0); camInfoA.P[0] = odom.data().stereoCameraModel().left().fx(); camInfoA.K[0] = odom.data().stereoCameraModel().left().fx(); camInfoA.P[5] = odom.data().stereoCameraModel().left().fy(); camInfoA.K[4] = odom.data().stereoCameraModel().left().fy(); camInfoA.P[2] = odom.data().stereoCameraModel().left().cx(); camInfoA.K[2] = odom.data().stereoCameraModel().left().cx(); camInfoA.P[6] = odom.data().stereoCameraModel().left().cy(); camInfoA.K[5] = odom.data().stereoCameraModel().left().cy(); camInfoB = camInfoA; camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx } type=1; if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1); if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1); if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1); if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1); } else { if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1); } camInfoA.height = odom.data().imageRaw().rows; camInfoA.width = odom.data().imageRaw().cols; camInfoB.height = odom.data().depthOrRightRaw().rows; camInfoB.width = odom.data().depthOrRightRaw().cols; if(!odom.data().laserScanRaw().empty()) { if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1); } // publish transforms first if(publishTf) { rtabmap::Transform localTransform; if(odom.data().cameraModels().size() == 1) { localTransform = odom.data().cameraModels()[0].localTransform(); } else if(odom.data().stereoCameraModel().isValidForProjection()) { localTransform = odom.data().stereoCameraModel().left().localTransform(); } if(!localTransform.isNull()) { geometry_msgs::TransformStamped baseToCamera; baseToCamera.child_frame_id = cameraFrameId; baseToCamera.header.frame_id = frameId; baseToCamera.header.stamp = time; rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform); tfBroadcaster.sendTransform(baseToCamera); } if(!odom.pose().isNull()) { geometry_msgs::TransformStamped odomToBase; odomToBase.child_frame_id = frameId; odomToBase.header.frame_id = odomFrameId; odomToBase.header.stamp = time; rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform); tfBroadcaster.sendTransform(odomToBase); } if(!scanPub.getTopic().empty()) { geometry_msgs::TransformStamped baseToLaserScan; baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.header.frame_id = frameId; baseToLaserScan.header.stamp = time; rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform); tfBroadcaster.sendTransform(baseToLaserScan); } } if(!odom.pose().isNull()) { if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1); if(odometryPub.getNumSubscribers()) { nav_msgs::Odometry odomMsg; odomMsg.child_frame_id = frameId; odomMsg.header.frame_id = odomFrameId; odomMsg.header.stamp = time; rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); UASSERT(odomMsg.pose.covariance.size() == 36 && odom.covariance().total() == 36 && odom.covariance().type() == CV_64FC1); memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); odometryPub.publish(odomMsg); } } if(type >= 0) { if(rgbCamInfoPub.getNumSubscribers() && type == 0) { rgbCamInfoPub.publish(camInfoA); } if(leftCamInfoPub.getNumSubscribers() && type == 1) { leftCamInfoPub.publish(camInfoA); } if(depthCamInfoPub.getNumSubscribers() && type == 0) { depthCamInfoPub.publish(camInfoB); } if(rightCamInfoPub.getNumSubscribers() && type == 1) { rightCamInfoPub.publish(camInfoB); } } if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers()) { cv_bridge::CvImage img; if(odom.data().imageRaw().channels() == 1) { img.encoding = sensor_msgs::image_encodings::MONO8; } else { img.encoding = sensor_msgs::image_encodings::BGR8; } img.image = odom.data().imageRaw(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.stamp = time; if(imagePub.getNumSubscribers()) { imagePub.publish(imageRosMsg); } if(rgbPub.getNumSubscribers() && type == 0) { rgbPub.publish(imageRosMsg); } if(leftPub.getNumSubscribers() && type == 1) { leftPub.publish(imageRosMsg); leftCamInfoPub.publish(camInfoA); } } if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0) { cv_bridge::CvImage img; if(odom.data().depthRaw().type() == CV_32FC1) { img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; } else { img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; } img.image = odom.data().depthRaw(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.stamp = time; depthPub.publish(imageRosMsg); depthCamInfoPub.publish(camInfoB); } if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1) { cv_bridge::CvImage img; img.encoding = sensor_msgs::image_encodings::MONO8; img.image = odom.data().rightRaw(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.stamp = time; rightPub.publish(imageRosMsg); rightCamInfoPub.publish(camInfoB); } if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty()) { //inspired from pointcloud_to_laserscan package sensor_msgs::LaserScan msg; msg.header.frame_id = scanFrameId; msg.header.stamp = time; msg.angle_min = scanAngleMin; msg.angle_max = scanAngleMax; msg.angle_increment = scanAngleIncrement; msg.time_increment = 0.0; msg.scan_time = scanTime; msg.range_min = scanRangeMin; msg.range_max = scanRangeMax; uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment); msg.ranges.assign(rangesSize, 0.0); const cv::Mat & scan = odom.data().laserScanRaw(); UASSERT(scan.type() == CV_32FC2 || scan.type() == CV_32FC3); UASSERT(scan.rows == 1); for (int i=0; i<scan.cols; ++i) { cv::Vec2f pos = scan.at<cv::Vec2f>(i); double range = hypot(pos[0], pos[1]); if (range >= scanRangeMin && range <=scanRangeMax) { double angle = atan2(pos[1], pos[0]); if (angle >= msg.angle_min && angle <= msg.angle_max) { int index = (angle - msg.angle_min) / msg.angle_increment; if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0)) { msg.ranges[index] = range; } } } } scanPub.publish(msg); } if(odom.data().userDataRaw().type() == CV_8SC1 && odom.data().userDataRaw().cols >= 7 && // including null str ending odom.data().userDataRaw().rows == 1 && memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) { //GOAL format detected, remove it from the user data and send it as goal event std::string goalStr = (const char *)odom.data().userDataRaw().data; if(!goalStr.empty()) { std::list<std::string> strs = uSplit(goalStr, ':'); if(strs.size() == 2) { int goalId = atoi(strs.rbegin()->c_str()); if(goalId > 0) { ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId); rtabmap_ros::SetGoal setGoalSrv; setGoalSrv.request.node_id = goalId; setGoalSrv.request.node_label = ""; if(!ros::service::call("set_goal", setGoalSrv)) { ROS_ERROR("Can't call \"set_goal\" service"); } } } } } ros::spinOnce(); while(ros::ok() && paused) { uSleep(100); ros::spinOnce(); } timer.restart(); info = rtabmap::CameraInfo(); data = reader.takeImage(&info); odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance); acquisitionTime = timer.ticks(); } return 0; }
int main(int argc, char * argv[]) { if(argc < 2) { showUsage(); } ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kDebug); std::string dictionaryPath = argv[argc-1]; std::list<std::vector<float> > objectDescriptors; //std::list<std::vector<float> > descriptors; std::map<int, std::vector<float> > descriptors; int dimension = 0; UTimer timer; int objectDescriptorsSize= 400; std::ifstream file; if(!dictionaryPath.empty()) { file.open(dictionaryPath.c_str(), std::ifstream::in); } if(file.good()) { UDEBUG("Loading the dictionary from \"%s\"", dictionaryPath.c_str()); // first line is the header std::string str; std::list<std::string> strList; std::getline(file, str); strList = uSplitNumChar(str); for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter) { if(uIsDigit(iter->at(0))) { dimension = std::atoi(iter->c_str()); break; } } if(dimension == 0 || dimension > 1000) { UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str()); } else { int descriptorsLoaded = 0; // Process all words while(file.good()) { std::getline(file, str); strList = uSplit(str); if((int)strList.size() == dimension+1) { //first one is the visual word id std::list<std::string>::iterator iter = strList.begin(); int id = atoi(iter->c_str()); ++iter; std::vector<float> descriptor(dimension); int i=0; //get descriptor for(;i<dimension && iter != strList.end(); ++i, ++iter) { descriptor[i] = uStr2Float(*iter); } if(i != dimension) { UERROR(""); } if(++descriptorsLoaded<=objectDescriptorsSize) { objectDescriptors.push_back(descriptor); } else { //descriptors.push_back(descriptor); descriptors.insert(std::make_pair(id, descriptor)); } } else if(str.size()) { UWARN("Cannot parse line \"%s\"", str.c_str()); } } } UDEBUG("Time loading dictionary = %fs, dimension=%d", timer.ticks(), dimension); } else { UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str()); } file.close(); if(descriptors.size() && objectDescriptors.size() && dimension) { cv::Mat dataTree; cv::Mat queries; UDEBUG("Creating data structures..."); // Create the data structure dataTree = cv::Mat((int)descriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F {//scope //std::list<std::vector<float> >::const_iterator iter = descriptors.begin(); std::map<int, std::vector<float> >::const_iterator iter = descriptors.begin(); for(unsigned int i=0; i < descriptors.size(); ++i, ++iter) { UTimer tim; //memcpy(dataTree.ptr<float>(i), iter->data(), dimension*sizeof(float)); memcpy(dataTree.ptr<float>(i), iter->second.data(), dimension*sizeof(float)); //if(i%100==0) // UDEBUG("i=%d/%d tim=%fs", i, descriptors.size(), tim.ticks()); } } queries = cv::Mat((int)objectDescriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F {//scope std::list<std::vector<float> >::const_iterator iter = objectDescriptors.begin(); for(unsigned int i=0; i < objectDescriptors.size(); ++i, ++iter) { UTimer tim; memcpy(queries.ptr<float>(i), iter->data(), dimension*sizeof(float)); //if(i%100==0) // UDEBUG("i=%d/%d tim=%fs", i, objectDescriptors.size(), tim.ticks()); } } UDEBUG("descriptors.size()=%d, objectDescriptorsSize=%d, copying data = %f s",descriptors.size(), objectDescriptors.size(), timer.ticks()); UDEBUG("Creating indexes..."); cv::flann::Index * linearIndex = new cv::flann::Index(dataTree, cv::flann::LinearIndexParams()); UDEBUG("Time to create linearIndex = %f s", timer.ticks()); cv::flann::Index * kdTreeIndex1 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(1)); UDEBUG("Time to create kdTreeIndex1 = %f s", timer.ticks()); cv::flann::Index * kdTreeIndex4 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(4)); UDEBUG("Time to create kdTreeIndex4 = %f s", timer.ticks()); cv::flann::Index * kMeansIndex = new cv::flann::Index(dataTree, cv::flann::KMeansIndexParams()); UDEBUG("Time to create kMeansIndex = %f s", timer.ticks()); cv::flann::Index * compositeIndex = new cv::flann::Index(dataTree, cv::flann::CompositeIndexParams()); UDEBUG("Time to create compositeIndex = %f s", timer.ticks()); //cv::flann::Index * autoTunedIndex = new cv::flann::Index(dataTree, cv::flann::AutotunedIndexParams()); //UDEBUG("Time to create autoTunedIndex = %f s", timer.ticks()); UDEBUG("Search indexes..."); int k=2; // 2 nearest neighbors cv::Mat results(queries.rows, k, CV_32SC1); // results index cv::Mat dists(queries.rows, k, CV_32FC1); // Distance results are CV_32FC1 linearIndex->knnSearch(queries, results, dists, k); //std::cout << results.t() << std::endl; cv::Mat transposedLinear = dists.t(); UDEBUG("Time to search linearIndex = %f s", timer.ticks()); kdTreeIndex1->knnSearch(queries, results, dists, k); //std::cout << results.t() << std::endl; cv::Mat transposed = dists.t(); UDEBUG("Time to search kdTreeIndex1 = %f s (size=%d, dist error(k1,k2)=(%f,%f))", timer.ticks(), transposed.cols, uMeanSquaredError( (float*)transposed.data, transposed.cols, (float*)transposedLinear.data, transposedLinear.cols), uMeanSquaredError( &transposed.at<float>(1,0), transposed.cols, &transposedLinear.at<float>(1,0), transposedLinear.cols)); kdTreeIndex4->knnSearch(queries, results, dists, k); //std::cout << results.t() << std::endl; transposed = dists.t(); UDEBUG("Time to search kdTreeIndex4 = %f s (size=%d, dist error(k1,k2)=(%f,%f))", timer.ticks(), transposed.cols, uMeanSquaredError( (float*)transposed.data, transposed.cols, (float*)transposedLinear.data, transposedLinear.cols), uMeanSquaredError( &transposed.at<float>(1,0), transposed.cols, &transposedLinear.at<float>(1,0), transposedLinear.cols)); kMeansIndex->knnSearch(queries, results, dists, k); //std::cout << results.t() << std::endl; transposed = dists.t(); UDEBUG("Time to search kMeansIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))", timer.ticks(), transposed.cols, uMeanSquaredError( (float*)transposed.data, transposed.cols, (float*)transposedLinear.data, transposedLinear.cols), uMeanSquaredError( &transposed.at<float>(1,0), transposed.cols, &transposedLinear.at<float>(1,0), transposedLinear.cols)); compositeIndex->knnSearch(queries, results, dists, k); //std::cout << results.t() << std::endl; transposed = dists.t(); UDEBUG("Time to search compositeIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))", timer.ticks(), transposed.cols, uMeanSquaredError( (float*)transposed.data, transposed.cols, (float*)transposedLinear.data, transposedLinear.cols), uMeanSquaredError( &transposed.at<float>(1,0), transposed.cols, &transposedLinear.at<float>(1,0), transposedLinear.cols)); //autoTunedIndex->knnSearch(queries, results, dists, k); //UDEBUG("Time to search autoTunedIndex = %f s", timer.ticks()); delete linearIndex; delete kdTreeIndex1; delete kdTreeIndex4; delete kMeansIndex; delete compositeIndex; //delete autoTunedIndex; } return 0; }