00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <ros/ros.h>
00029 #include "rtabmap_ros/MapData.h"
00030 #include "rtabmap_ros/MsgConversion.h"
00031 #include "rtabmap_ros/MapsManager.h"
00032 #include <rtabmap/core/util3d_transforms.h>
00033 #include <rtabmap/core/util3d.h>
00034 #include <rtabmap/core/util3d_filtering.h>
00035 #include <rtabmap/core/util3d_mapping.h>
00036 #include <rtabmap/core/Compression.h>
00037 #include <rtabmap/core/Graph.h>
00038 #include <rtabmap/utilite/ULogger.h>
00039 #include <rtabmap/utilite/UStl.h>
00040 #include <rtabmap/utilite/UTimer.h>
00041 #include <rtabmap/utilite/UFile.h>
00042 #include <pcl_ros/transforms.h>
00043 #include <pcl_conversions/pcl_conversions.h>
00044 #include <nav_msgs/OccupancyGrid.h>
00045 #include <std_srvs/Empty.h>
00046
00047 using namespace rtabmap;
00048
00049 class MapAssembler
00050 {
00051
00052 public:
00053 MapAssembler(int & argc, char** argv)
00054 {
00055 ros::NodeHandle pnh("~");
00056 ros::NodeHandle nh;
00057
00058 std::string configPath;
00059 pnh.param("config_path", configPath, configPath);
00060
00061
00062 rtabmap::ParametersMap parameters;
00063 uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
00064 uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
00065 if(!configPath.empty())
00066 {
00067 if(UFile::exists(configPath.c_str()))
00068 {
00069 ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
00070 rtabmap::ParametersMap allParameters;
00071 Parameters::readINI(configPath.c_str(), allParameters);
00072
00073 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00074 {
00075 ParametersMap::iterator jter = allParameters.find(iter->first);
00076 if(jter!=allParameters.end())
00077 {
00078 iter->second = jter->second;
00079 }
00080 }
00081 }
00082 else
00083 {
00084 ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
00085 }
00086 }
00087 for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00088 {
00089 std::string vStr;
00090 bool vBool;
00091 int vInt;
00092 double vDouble;
00093 if(pnh.getParam(iter->first, vStr))
00094 {
00095 ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
00096 iter->second = vStr;
00097 }
00098 else if(pnh.getParam(iter->first, vBool))
00099 {
00100 ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
00101 iter->second = uBool2Str(vBool);
00102 }
00103 else if(pnh.getParam(iter->first, vDouble))
00104 {
00105 ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
00106 iter->second = uNumber2Str(vDouble);
00107 }
00108 else if(pnh.getParam(iter->first, vInt))
00109 {
00110 ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
00111 iter->second = uNumber2Str(vInt);
00112 }
00113
00114 if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
00115 {
00116 ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
00117 iter->second = uNumber2Str(8);
00118 }
00119 }
00120
00121 rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
00122 for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
00123 {
00124 rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
00125 if(jter!=parameters.end())
00126 {
00127 ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
00128 jter->second = iter->second;
00129 }
00130 }
00131
00132
00133 for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
00134 iter!=Parameters::getRemovedParameters().end();
00135 ++iter)
00136 {
00137 std::string vStr;
00138 if(pnh.getParam(iter->first, vStr))
00139 {
00140 if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
00141 {
00142
00143 parameters.at(iter->second.second)= vStr;
00144 ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
00145 ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
00146 }
00147 else
00148 {
00149 if(iter->second.second.empty())
00150 {
00151 ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
00152 ros::this_node::getName().c_str(), iter->first.c_str());
00153 }
00154 else
00155 {
00156 ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
00157 ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
00158 }
00159 }
00160 }
00161 }
00162
00163 mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
00164 mapsManager_.backwardCompatibilityParameters(pnh, parameters);
00165 mapsManager_.setParameters(parameters);
00166
00167 mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
00168
00169
00170 resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
00171 }
00172
00173 ~MapAssembler()
00174 {
00175 }
00176
00177 void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
00178 {
00179 UTimer timer;
00180
00181 std::map<int, Transform> poses;
00182 std::multimap<int, Link> constraints;
00183 Transform mapOdom;
00184 rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
00185 for(unsigned int i=0; i<msg->nodes.size(); ++i)
00186 {
00187 if(msg->nodes[i].image.size() ||
00188 msg->nodes[i].depth.size() ||
00189 msg->nodes[i].laserScan.size())
00190 {
00191 uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
00192 }
00193 }
00194
00195
00196 if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
00197 {
00198 Signature tmpS = nodes_.at(poses.rbegin()->first);
00199 SensorData tmpData = tmpS.sensorData();
00200 tmpData.setId(-1);
00201 uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
00202 poses.insert(std::make_pair(-1, poses.rbegin()->second));
00203 }
00204
00205
00206 poses = mapsManager_.updateMapCaches(
00207 poses,
00208 0,
00209 false,
00210 false,
00211 nodes_);
00212
00213 mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
00214
00215 ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
00216 }
00217
00218 bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00219 {
00220 ROS_INFO("map_assembler: reset!");
00221 mapsManager_.clear();
00222 return true;
00223 }
00224
00225 private:
00226 MapsManager mapsManager_;
00227 std::map<int, Signature> nodes_;
00228
00229 ros::Subscriber mapDataTopic_;
00230
00231 ros::ServiceServer resetService_;
00232 };
00233
00234
00235 int main(int argc, char** argv)
00236 {
00237 ros::init(argc, argv, "map_assembler");
00238
00239
00240 for(int i=1;i<argc;++i)
00241 {
00242 if(strcmp(argv[i], "--params") == 0)
00243 {
00244 rtabmap::ParametersMap parameters;
00245 uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
00246 uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
00247 for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00248 {
00249 std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
00250 std::cout <<
00251 str <<
00252 std::setw(60 - str.size()) <<
00253 " [" <<
00254 rtabmap::Parameters::getDescription(iter->first).c_str() <<
00255 "]" <<
00256 std::endl;
00257 }
00258 ROS_WARN("Node will now exit after showing default parameters because "
00259 "argument \"--params\" is detected!");
00260 exit(0);
00261 }
00262 else if(strcmp(argv[i], "--udebug") == 0)
00263 {
00264 ULogger::setLevel(ULogger::kDebug);
00265 }
00266 else if(strcmp(argv[i], "--uinfo") == 0)
00267 {
00268 ULogger::setLevel(ULogger::kInfo);
00269 }
00270 }
00271
00272 MapAssembler assembler(argc, argv);
00273 ros::spin();
00274 return 0;
00275 }