MapAssemblerNode.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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                 //parameters
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                                 // only update odometry parameters
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                 // Backward compatibility
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                                         // can be migrated
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                 // private service
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                 // create a tmp signature with latest sensory data
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                 // Update maps
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         // process "--params" argument
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 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:30:49