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  localGridsRegenerated_(false)
55  {
56  ros::NodeHandle pnh("~");
57  ros::NodeHandle nh;
58 
59  std::string configPath;
60  pnh.param("config_path", configPath, configPath);
61  pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_);
62 
63  //parameters
64  rtabmap::ParametersMap parameters;
65  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
66  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
67  if(!configPath.empty())
68  {
69  if(UFile::exists(configPath.c_str()))
70  {
71  ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
72  rtabmap::ParametersMap allParameters;
73  Parameters::readINI(configPath.c_str(), allParameters);
74  // only update odometry parameters
75  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
76  {
77  ParametersMap::iterator jter = allParameters.find(iter->first);
78  if(jter!=allParameters.end())
79  {
80  iter->second = jter->second;
81  }
82  }
83  }
84  else
85  {
86  ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
87  }
88  }
89  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
90  {
91  std::string vStr;
92  bool vBool;
93  int vInt;
94  double vDouble;
95  if(pnh.getParam(iter->first, vStr))
96  {
97  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
98  iter->second = vStr;
99  }
100  else if(pnh.getParam(iter->first, vBool))
101  {
102  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
103  iter->second = uBool2Str(vBool);
104  }
105  else if(pnh.getParam(iter->first, vDouble))
106  {
107  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
108  iter->second = uNumber2Str(vDouble);
109  }
110  else if(pnh.getParam(iter->first, vInt))
111  {
112  ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
113  iter->second = uNumber2Str(vInt);
114  }
115 
116  if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
117  {
118  ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
119  iter->second = uNumber2Str(8);
120  }
121  }
122 
123  rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
124  for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
125  {
126  rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
127  if(jter!=parameters.end())
128  {
129  ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
130  jter->second = iter->second;
131  }
132  }
133 
134  // Backward compatibility
135  for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
136  iter!=Parameters::getRemovedParameters().end();
137  ++iter)
138  {
139  std::string vStr;
140  if(pnh.getParam(iter->first, vStr))
141  {
142  if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
143  {
144  // can be migrated
145  parameters.at(iter->second.second)= vStr;
146  ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
147  ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
148  }
149  else
150  {
151  if(iter->second.second.empty())
152  {
153  ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
154  ros::this_node::getName().c_str(), iter->first.c_str());
155  }
156  else
157  {
158  ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
159  ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
160  }
161  }
162  }
163  }
164 
165  mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
166  mapsManager_.backwardCompatibilityParameters(pnh, parameters);
167  mapsManager_.setParameters(parameters);
168 
169  mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
170 
171  // private service
172  resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
173  }
174 
175  ~MapAssembler()
176  {
177  }
178 
179  void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
180  {
181  UTimer timer;
182 
183  std::map<int, Transform> poses;
184  std::multimap<int, Link> constraints;
185  Transform mapOdom;
186  rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
187  for(unsigned int i=0; i<msg->nodes.size(); ++i)
188  {
189  if(msg->nodes[i].image.size() ||
190  msg->nodes[i].depth.size() ||
191  msg->nodes[i].laserScan.size())
192  {
193  Signature data = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
194  if(localGridsRegenerated_)
195  {
196  data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
197  }
198  uInsert(nodes_, std::make_pair(msg->nodes[i].id, data));
199  }
200  }
201 
202  // create a tmp signature with latest sensory data
203  if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
204  {
205  Signature tmpS = nodes_.at(poses.rbegin()->first);
206  SensorData tmpData = tmpS.sensorData();
207  tmpData.setId(0);
208  uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
209  poses.insert(std::make_pair(0, poses.rbegin()->second));
210  }
211 
212  // Update maps
213  poses = mapsManager_.updateMapCaches(
214  poses,
215  0,
216  false,
217  false,
218  nodes_);
219 
220  mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
221 
222  ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
223  }
224 
225  bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
226  {
227  ROS_INFO("map_assembler: reset!");
228  mapsManager_.clear();
229  return true;
230  }
231 
232 private:
233  MapsManager mapsManager_;
234  std::map<int, Signature> nodes_;
235 
236  ros::Subscriber mapDataTopic_;
237 
238  ros::ServiceServer resetService_;
239 
240  bool localGridsRegenerated_;
241 };
242 
243 
244 int main(int argc, char** argv)
245 {
246  ros::init(argc, argv, "map_assembler");
247 
248  // process "--params" argument
249  for(int i=1;i<argc;++i)
250  {
251  if(strcmp(argv[i], "--params") == 0)
252  {
253  rtabmap::ParametersMap parameters;
254  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
255  uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
256  for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
257  {
258  std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
259  std::cout <<
260  str <<
261  std::setw(60 - str.size()) <<
262  " [" <<
263  rtabmap::Parameters::getDescription(iter->first).c_str() <<
264  "]" <<
265  std::endl;
266  }
267  ROS_WARN("Node will now exit after showing default parameters because "
268  "argument \"--params\" is detected!");
269  exit(0);
270  }
271  else if(strcmp(argv[i], "--udebug") == 0)
272  {
273  ULogger::setLevel(ULogger::kDebug);
274  }
275  else if(strcmp(argv[i], "--uinfo") == 0)
276  {
277  ULogger::setLevel(ULogger::kInfo);
278  }
279  }
280 
281  MapAssembler assembler(argc, argv);
282  ros::spin();
283  return 0;
284 }
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:1016
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:233
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:225
util3d_mapping.h
main
int main(int argc, char **argv)
Definition: MapAssemblerNode.cpp:244
rtabmap::Signature::getStamp
double getStamp() const
MapAssembler::nodes_
std::map< int, Signature > nodes_
Definition: MapAssemblerNode.cpp:234
uNumber2Str
std::string uNumber2Str(unsigned int number)
MapAssembler::resetService_
ros::ServiceServer resetService_
Definition: MapAssemblerNode.cpp:238
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:966
util3d_transforms.h
false
#define false
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:179
MapAssembler::mapDataTopic_
ros::Subscriber mapDataTopic_
Definition: MapAssemblerNode.cpp:236
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19