MapAssemblerNode.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include "rtabmap_ros/MapData.h"
30 #include "rtabmap_ros/MsgConversion.h"
31 #include "rtabmap_ros/MapsManager.h"
32 #include <rtabmap/core/util3d_transforms.h>
33 #include <rtabmap/core/util3d.h>
34 #include <rtabmap/core/util3d_filtering.h>
35 #include <rtabmap/core/util3d_mapping.h>
36 #include <rtabmap/core/Compression.h>
37 #include <rtabmap/core/Graph.h>
38 #include <rtabmap/utilite/ULogger.h>
39 #include <rtabmap/utilite/UStl.h>
40 #include <rtabmap/utilite/UTimer.h>
41 #include <rtabmap/utilite/UFile.h>
42 #include <pcl_ros/transforms.h>
43 #include <pcl_conversions/pcl_conversions.h>
44 #include <nav_msgs/OccupancyGrid.h>
45 #include <std_srvs/Empty.h>
46 
47 using namespace rtabmap;
48 
49 class MapAssembler
50 {
51 
52 public:
53  MapAssembler(int & argc, char** argv)
54  {
55  ros::NodeHandle pnh("~");
56  ros::NodeHandle nh;
57 
58  std::string configPath;
59  pnh.param("config_path", configPath, configPath);
60 
61  //parameters
62  rtabmap::ParametersMap parameters;
63  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
64  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
65  if(!configPath.empty())
66  {
67  if(UFile::exists(configPath.c_str()))
68  {
69  ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
70  rtabmap::ParametersMap allParameters;
71  Parameters::readINI(configPath.c_str(), allParameters);
72  // only update odometry parameters
73  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
74  {
75  ParametersMap::iterator jter = allParameters.find(iter->first);
76  if(jter!=allParameters.end())
77  {
78  iter->second = jter->second;
79  }
80  }
81  }
82  else
83  {
84  ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
85  }
86  }
87  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
88  {
89  std::string vStr;
90  bool vBool;
91  int vInt;
92  double vDouble;
93  if(pnh.getParam(iter->first, vStr))
94  {
95  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
96  iter->second = vStr;
97  }
98  else if(pnh.getParam(iter->first, vBool))
99  {
100  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
101  iter->second = uBool2Str(vBool);
102  }
103  else if(pnh.getParam(iter->first, vDouble))
104  {
105  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
106  iter->second = uNumber2Str(vDouble);
107  }
108  else if(pnh.getParam(iter->first, vInt))
109  {
110  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
111  iter->second = uNumber2Str(vInt);
112  }
113 
114  if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
115  {
116  ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
117  iter->second = uNumber2Str(8);
118  }
119  }
120 
121  rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
122  for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
123  {
124  rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
125  if(jter!=parameters.end())
126  {
127  ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
128  jter->second = iter->second;
129  }
130  }
131 
132  // Backward compatibility
133  for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
134  iter!=Parameters::getRemovedParameters().end();
135  ++iter)
136  {
137  std::string vStr;
138  if(pnh.getParam(iter->first, vStr))
139  {
140  if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
141  {
142  // can be migrated
143  parameters.at(iter->second.second)= vStr;
144  ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
145  ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
146  }
147  else
148  {
149  if(iter->second.second.empty())
150  {
151  ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
152  ros::this_node::getName().c_str(), iter->first.c_str());
153  }
154  else
155  {
156  ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
157  ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
158  }
159  }
160  }
161  }
162 
163  mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
164  mapsManager_.backwardCompatibilityParameters(pnh, parameters);
165  mapsManager_.setParameters(parameters);
166 
167  mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
168 
169  // private service
170  resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
171  }
172 
173  ~MapAssembler()
174  {
175  }
176 
177  void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
178  {
179  UTimer timer;
180 
181  std::map<int, Transform> poses;
182  std::multimap<int, Link> constraints;
183  Transform mapOdom;
184  rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
185  for(unsigned int i=0; i<msg->nodes.size(); ++i)
186  {
187  if(msg->nodes[i].image.size() ||
188  msg->nodes[i].depth.size() ||
189  msg->nodes[i].laserScan.size())
190  {
191  uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
192  }
193  }
194 
195  // create a tmp signature with latest sensory data
196  if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
197  {
198  Signature tmpS = nodes_.at(poses.rbegin()->first);
199  SensorData tmpData = tmpS.sensorData();
200  tmpData.setId(-1);
201  uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
202  poses.insert(std::make_pair(-1, poses.rbegin()->second));
203  }
204 
205  // Update maps
206  poses = mapsManager_.updateMapCaches(
207  poses,
208  0,
209  false,
210  false,
211  nodes_);
212 
213  mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
214 
215  ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
216  }
217 
218  bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
219  {
220  ROS_INFO("map_assembler: reset!");
221  mapsManager_.clear();
222  return true;
223  }
224 
225 private:
226  MapsManager mapsManager_;
227  std::map<int, Signature> nodes_;
228 
229  ros::Subscriber mapDataTopic_;
230 
231  ros::ServiceServer resetService_;
232 };
233 
234 
235 int main(int argc, char** argv)
236 {
237  ros::init(argc, argv, "map_assembler");
238 
239  // process "--params" argument
240  for(int i=1;i<argc;++i)
241  {
242  if(strcmp(argv[i], "--params") == 0)
243  {
244  rtabmap::ParametersMap parameters;
245  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
246  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
247  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
248  {
249  std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
250  std::cout <<
251  str <<
252  std::setw(60 - str.size()) <<
253  " [" <<
254  rtabmap::Parameters::getDescription(iter->first).c_str() <<
255  "]" <<
256  std::endl;
257  }
258  ROS_WARN("Node will now exit after showing default parameters because "
259  "argument \"--params\" is detected!");
260  exit(0);
261  }
262  else if(strcmp(argv[i], "--udebug") == 0)
263  {
264  ULogger::setLevel(ULogger::kDebug);
265  }
266  else if(strcmp(argv[i], "--uinfo") == 0)
267  {
268  ULogger::setLevel(ULogger::kInfo);
269  }
270  }
271 
272  MapAssembler assembler(argc, argv);
273  ros::spin();
274  return 0;
275 }
rtabmap::Parameters::getRemovedParameters
static const std::map< std::string, std::pair< bool, std::string > > & getRemovedParameters()
ros::NodeHandle
rtabmap::Parameters::getDescription
static std::string getDescription(const std::string &paramKey)
rtabmap_ros::nodeDataFromROS
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData &msg)
Definition: MsgConversion.cpp:787
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
MapAssembler::mapsManager_
MapsManager mapsManager_
Definition: MapAssemblerNode.cpp:226
rtabmap::SensorData::setId
void setId(int id)
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ULogger::setLevel
static void setLevel(ULogger::Level level)
ROS_WARN
#define ROS_WARN(...)
uBool2Str
std::string uBool2Str(bool boolean)
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
MapAssembler::reset
bool reset(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: MapAssemblerNode.cpp:218
util3d_mapping.h
main
int main(int argc, char **argv)
Definition: MapAssemblerNode.cpp:235
rtabmap::Signature::getStamp
double getStamp() const
MapAssembler::nodes_
std::map< int, Signature > nodes_
Definition: MapAssemblerNode.cpp:227
uNumber2Str
std::string uNumber2Str(unsigned int number)
MapAssembler::resetService_
ros::ServiceServer resetService_
Definition: MapAssemblerNode.cpp:231
util3d_filtering.h
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
pcl_conversions.h
rtabmap_ros::mapGraphFromROS
void mapGraphFromROS(const rtabmap_ros::MapGraph &msg, std::map< int, rtabmap::Transform > &poses, std::multimap< int, rtabmap::Link > &links, rtabmap::Transform &mapToOdom)
Definition: MsgConversion.cpp:737
util3d_transforms.h
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
rtabmap::Signature::getPose
const Transform & getPose() const
rtabmap::Signature::sensorData
SensorData & sensorData()
UFile::exists
bool exists()
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
UTimer::ticks
double ticks()
MapAssembler::MapAssembler
MapAssembler(int &argc, char **argv)
Definition: MapAssemblerNode.cpp:53
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
ROS_ERROR
#define ROS_ERROR(...)
MapAssembler::mapDataReceivedCallback
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr &msg)
Definition: MapAssemblerNode.cpp:177
MapAssembler::mapDataTopic_
ros::Subscriber mapDataTopic_
Definition: MapAssemblerNode.cpp:229
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03