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_ros/GetMap.h"
33 #include <rtabmap/core/util3d_transforms.h>
34 #include <rtabmap/core/util3d.h>
35 #include <rtabmap/core/util3d_filtering.h>
36 #include <rtabmap/core/util3d_mapping.h>
37 #include <rtabmap/core/Compression.h>
38 #include <rtabmap/core/Graph.h>
39 #include <rtabmap/utilite/ULogger.h>
40 #include <rtabmap/utilite/UStl.h>
41 #include <rtabmap/utilite/UTimer.h>
42 #include <rtabmap/utilite/UFile.h>
43 #include <pcl_ros/transforms.h>
44 #include <pcl_conversions/pcl_conversions.h>
45 #include <nav_msgs/OccupancyGrid.h>
46 #include <std_srvs/Empty.h>
47 
48 #ifdef WITH_OCTOMAP_MSGS
49 #ifdef RTABMAP_OCTOMAP
50 #include <octomap_msgs/GetOctomap.h>
51 #include <octomap_msgs/conversions.h>
52 #include <rtabmap/core/OctoMap.h>
53 #endif
54 #endif
55 
56 using namespace rtabmap;
57 
58 class MapAssembler
59 {
60 
61 public:
62  MapAssembler(int & argc, char** argv) :
63  localGridsRegenerated_(false)
64  {
65  ros::NodeHandle pnh("~");
66  ros::NodeHandle nh;
67 
68  std::string configPath;
69  pnh.param("config_path", configPath, configPath);
70  pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_);
71 
72  //parameters
73  rtabmap::ParametersMap parameters;
74  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
75  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
76  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
77  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
78  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
79  if(!configPath.empty())
80  {
81  if(UFile::exists(configPath.c_str()))
82  {
83  ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
84  rtabmap::ParametersMap allParameters;
85  Parameters::readINI(configPath.c_str(), allParameters);
86  // only update odometry parameters
87  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
88  {
89  ParametersMap::iterator jter = allParameters.find(iter->first);
90  if(jter!=allParameters.end())
91  {
92  iter->second = jter->second;
93  }
94  }
95  }
96  else
97  {
98  ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
99  }
100  }
101  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
102  {
103  std::string vStr;
104  bool vBool;
105  int vInt;
106  double vDouble;
107  if(pnh.getParam(iter->first, vStr))
108  {
109  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
110  iter->second = vStr;
111  }
112  else if(pnh.getParam(iter->first, vBool))
113  {
114  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
115  iter->second = uBool2Str(vBool);
116  }
117  else if(pnh.getParam(iter->first, vDouble))
118  {
119  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
120  iter->second = uNumber2Str(vDouble);
121  }
122  else if(pnh.getParam(iter->first, vInt))
123  {
124  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
125  iter->second = uNumber2Str(vInt);
126  }
127  }
128 
129  rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
130  for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
131  {
132  rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
133  if(jter!=parameters.end())
134  {
135  ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
136  jter->second = iter->second;
137  }
138  }
139 
140  // Backward compatibility
141  for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
142  iter!=Parameters::getRemovedParameters().end();
143  ++iter)
144  {
145  std::string vStr;
146  if(pnh.getParam(iter->first, vStr))
147  {
148  if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
149  {
150  // can be migrated
151  parameters.at(iter->second.second)= vStr;
152  ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
153  ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
154  }
155  else
156  {
157  if(iter->second.second.empty())
158  {
159  ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
160  ros::this_node::getName().c_str(), iter->first.c_str());
161  }
162  else
163  {
164  ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
165  ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
166  }
167  }
168  }
169  }
170 
171  // set private parameters
172  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
173  {
174  pnh.setParam(iter->first, iter->second);
175  }
176 
177  ROS_INFO("%s: regenerate_local_grids = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
178  mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
179  mapsManager_.backwardCompatibilityParameters(pnh, parameters);
180  mapsManager_.setParameters(parameters);
181 
182  std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
183  std::string rtabmapNs;
184  for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
185  {
186  if(!rtabmapNs.empty())
187  {
188  rtabmapNs += "/";
189  }
190  rtabmapNs += *iter;
191  }
192  ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
193  if(rtabmapNs.empty())
194  {
195  rtabmapNs = "get_map_data";
196  }
197  else
198  {
199  rtabmapNs += "/get_map_data";
200  }
201 
202  rtabmap_ros::GetMap getMapSrv;
203  getMapSrv.request.global = false;
204  getMapSrv.request.optimized = true;
205  getMapSrv.request.graphOnly = false;
206  if(ros::service::waitForService(rtabmapNs, 5000))
207  {
208  if(!ros::service::call(rtabmapNs, getMapSrv))
209  {
210  ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
211  }
212  else
213  {
214  ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
215  processMapData(getMapSrv.response.data);
216  ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
217  " will be assembled on next subscriber connection.", rtabmapNs.c_str());
218  }
219  }
220  else
221  {
222  ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
223  "may not be a problem if rtabmap is started afterwards. If rtabmap "
224  "is started after in localization mode, call /rtabmap/publish_maps "
225  "service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
226  }
227 
228 
229  mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
230 
231  // private services
232  resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
233 
234 #ifdef WITH_OCTOMAP_MSGS
235 #ifdef RTABMAP_OCTOMAP
236  octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
237  octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
238 #endif
239 #endif
240  }
241 
242  ~MapAssembler()
243  {
244  }
245 
246  void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
247  {
248  processMapData(*msg);
249  }
250  void processMapData(const rtabmap_ros::MapData & msg)
251  {
252  UTimer timer;
253 
254  std::map<int, Transform> poses;
255  std::multimap<int, Link> constraints;
256  Transform mapOdom;
257  rtabmap_ros::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
258  for(unsigned int i=0; i<msg.nodes.size(); ++i)
259  {
260  if(msg.nodes[i].image.size() ||
261  msg.nodes[i].depth.size() ||
262  msg.nodes[i].laserScan.size())
263  {
264  Signature data = rtabmap_ros::nodeDataFromROS(msg.nodes[i]);
265  if(localGridsRegenerated_)
266  {
267  data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
268  }
269  uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
270  }
271  }
272 
273  // create a tmp signature with latest sensory data
274  if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
275  {
276  Signature tmpS = nodes_.at(poses.rbegin()->first);
277  SensorData tmpData = tmpS.sensorData();
278  tmpData.setId(0);
279  uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
280  poses.insert(std::make_pair(0, poses.rbegin()->second));
281  }
282 
283  // Update maps
284  if(!nodes_.empty())
285  {
286  poses = mapsManager_.updateMapCaches(
287  poses,
288  0,
289  false,
290  false,
291  nodes_);
292  }
293  double updateTime = timer.ticks();
294 
295  mapFrameId_ = msg.header.frame_id;
296  optimizedPoses_ = poses;
297 
298  mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
299 
300  ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
301  }
302 
303  bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
304  {
305  ROS_INFO("map_assembler: reset!");
306  mapsManager_.clear();
307  return true;
308  }
309 
310 #ifdef WITH_OCTOMAP_MSGS
311 #ifdef RTABMAP_OCTOMAP
312  bool octomapBinaryCallback(
313  octomap_msgs::GetOctomap::Request &req,
314  octomap_msgs::GetOctomap::Response &res)
315  {
316  ROS_INFO("Sending binary map data on service request");
317  res.map.header.frame_id = mapFrameId_;
318  res.map.header.stamp = ros::Time::now();
319 
320  mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
321 
322  const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
323  bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
324  return success;
325  }
326 
327  bool octomapFullCallback(
328  octomap_msgs::GetOctomap::Request &req,
329  octomap_msgs::GetOctomap::Response &res)
330  {
331  ROS_INFO("Sending full map data on service request");
332  res.map.header.frame_id = mapFrameId_;
333  res.map.header.stamp = ros::Time::now();
334 
335  mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
336 
337  const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
338  bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
339  return success;
340  }
341 #endif
342 #endif
343 
344 private:
345  MapsManager mapsManager_;
346  std::map<int, Signature> nodes_;
347  std::map<int, Transform> optimizedPoses_;
348  std::string mapFrameId_;
349 
350  ros::Subscriber mapDataTopic_;
351 
352  ros::ServiceServer resetService_;
353 #ifdef WITH_OCTOMAP_MSGS
354 #ifdef RTABMAP_OCTOMAP
355  ros::ServiceServer octomapBinarySrv_;
356  ros::ServiceServer octomapFullSrv_;
357 #endif
358 #endif
359  bool localGridsRegenerated_;
360 };
361 
362 
363 int main(int argc, char** argv)
364 {
365  ULogger::setLevel(ULogger::kError);
366  ULogger::setType(ULogger::kTypeConsole);
367 
368  ros::init(argc, argv, "map_assembler");
369 
370  // process "--params" argument
371  for(int i=1;i<argc;++i)
372  {
373  if(strcmp(argv[i], "--params") == 0)
374  {
375  rtabmap::ParametersMap parameters;
376  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
377  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
378  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
379  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
380  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
381  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
382  {
383  std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
384  std::cout <<
385  str <<
386  std::setw(60 - str.size()) <<
387  " [" <<
388  rtabmap::Parameters::getDescription(iter->first).c_str() <<
389  "]" <<
390  std::endl;
391  }
392  ROS_WARN("Node will now exit after showing default parameters because "
393  "argument \"--params\" is detected!");
394  exit(0);
395  }
396  else if(strcmp(argv[i], "--udebug") == 0)
397  {
398  ULogger::setLevel(ULogger::kDebug);
399  }
400  else if(strcmp(argv[i], "--uinfo") == 0)
401  {
402  ULogger::setLevel(ULogger::kInfo);
403  }
404  }
405 
406  MapAssembler assembler(argc, argv);
407  ros::spin();
408  return 0;
409 }
getMapSrv
ros::ServiceClient getMapSrv
Definition: ExternalLoopDetectionExample.cpp:62
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::SensorData::setOccupancyGrid
void setOccupancyGrid(const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint)
rtabmap_ros::nodeDataFromROS
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData &msg)
Definition: MsgConversion.cpp:1064
MapAssembler::mapFrameId_
std::string mapFrameId_
Definition: MapAssemblerNode.cpp:348
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::ParametersPair
std::pair< std::string, std::string > ParametersPair
ros::service::call
bool call(const std::string &service_name, MReq &req, MRes &res)
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
data
data
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:345
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(...)
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
uBool2Str
std::string uBool2Str(bool boolean)
MapAssembler::optimizedPoses_
std::map< int, Transform > optimizedPoses_
Definition: MapAssemblerNode.cpp:347
MapAssembler::reset
bool reset(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: MapAssemblerNode.cpp:303
util3d_mapping.h
main
int main(int argc, char **argv)
Definition: MapAssemblerNode.cpp:363
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
MapAssembler::nodes_
std::map< int, Signature > nodes_
Definition: MapAssemblerNode.cpp:346
uNumber2Str
std::string uNumber2Str(unsigned int number)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
MapAssembler::resetService_
ros::ServiceServer resetService_
Definition: MapAssemblerNode.cpp:352
util3d_filtering.h
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
pcl_conversions.h
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
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:1014
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
ros::spin
ROSCPP_DECL void spin()
util3d_transforms.h
false
#define false
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
MapAssembler::processMapData
void processMapData(const rtabmap_ros::MapData &msg)
Definition: MapAssemblerNode.cpp:250
rtabmap::Signature::sensorData
SensorData & sensorData()
ros::NodeHandle::setParam
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
UFile::exists
bool exists()
rtabmap::Signature::getPose
const Transform & getPose() const
ros::Time::now
static Time now()
UTimer::ticks
double ticks()
rtabmap::Signature::getStamp
double getStamp() const
MapAssembler::MapAssembler
MapAssembler(int &argc, char **argv)
Definition: MapAssemblerNode.cpp:62
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:246
MapAssembler::mapDataTopic_
ros::Subscriber mapDataTopic_
Definition: MapAssemblerNode.cpp:350
res
res
ros::service::waitForService
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40