merge_maps_kinematic.cpp
Go to the documentation of this file.
1 /*
2  * Author
3  * Copyright (c) 2018, Simbe Robotics, Inc.
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
21 
22 /*****************************************************************************/
23 MergeMapsKinematic::MergeMapsKinematic() : num_submaps_(0), nh_("map_merging")
24 /*****************************************************************************/
25 {
26  ROS_INFO("MergeMapsKinematic: Starting up!");
27  setup();
28 }
29 
30 /*****************************************************************************/
32 /*****************************************************************************/
33 {
34  nh_.param("resolution", resolution_, 0.05);
35  sstS_.push_back(nh_.advertise<nav_msgs::OccupancyGrid>("/map", 1, true));
36  sstmS_.push_back(nh_.advertise<nav_msgs::MapMetaData>(
37  "/map_metadata", 1, true));
38  ssMap_ = nh_.advertiseService("merge_submaps",
40  ssSubmap_ = nh_.advertiseService("add_submap",
42 
43  tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
44 
46  std::make_unique<interactive_markers::InteractiveMarkerServer>(
47  "merge_maps_tool","",true);
48 }
49 
50 /*****************************************************************************/
52 /*****************************************************************************/
53 {
54 }
55 
56 /*****************************************************************************/
58  slam_toolbox_msgs::AddSubmap::Request& req,
59  slam_toolbox_msgs::AddSubmap::Response& resp)
60 /*****************************************************************************/
61 {
62  std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();
63  std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
64 
65  if (!serialization::read(req.filename, *mapper, *dataset))
66  {
67  ROS_ERROR("addSubmapCallback: Failed to read "
68  "file: %s.", req.filename.c_str());
69  return true;
70  }
71 
72  // we know the position because we put it there before any scans
73  karto::LaserRangeFinder* laser = dynamic_cast<karto::LaserRangeFinder*>(
74  dataset->GetLasers()[0]);
75  dataset->Add(laser, true);
76  dataset_vec_.push_back(std::move(dataset));
77 
78  if (lasers_.find(laser->GetName().GetName()) == lasers_.end())
79  {
80  laser_utils::LaserMetadata laserMeta(laser, false);
81  lasers_[laser->GetName().GetName()] = laserMeta;
82  }
83 
84  karto::LocalizedRangeScanVector scans = mapper->GetAllProcessedScans();
85  scans_vec_.push_back(scans);
86  num_submaps_++;
87 
88  // create and publish map with marker that will move the map around
89  sstS_.push_back(nh_.advertise<nav_msgs::OccupancyGrid>(
90  "/map_"+std::to_string(num_submaps_), 1, true));
91  sstmS_.push_back(nh_.advertise<nav_msgs::MapMetaData>(
92  "/map_metadata_" + std::to_string(num_submaps_), 1, true));
93  sleep(1.0);
94 
95  nav_msgs::GetMap::Response map;
96  nav_msgs::OccupancyGrid& og = map.map;
97  try
98  {
99  kartoToROSOccupancyGrid(scans, map);
100  } catch (const karto::Exception& e)
101  {
102  ROS_WARN("Failed to build grid to add submap, Exception: %s",
103  e.GetErrorMessage().c_str());
104  return false;
105  }
106 
107  tf2::Transform transform;
108  transform.setIdentity();
109  transform.setOrigin(tf2::Vector3(og.info.origin.position.x +
110  og.info.width * og.info.resolution / 2.0,
111  og.info.origin.position.y + og.info.height * og.info.resolution / 2.0,
112  0.));
113  og.info.origin.position.x = - (og.info.width * og.info.resolution / 2.0);
114  og.info.origin.position.y = - (og.info.height * og.info.resolution / 2.0);
115  og.header.stamp = ros::Time::now();
116  og.header.frame_id = "map_"+std::to_string(num_submaps_);
117  sstS_[num_submaps_].publish(og);
118  sstmS_[num_submaps_].publish(og.info);
119 
120  geometry_msgs::TransformStamped msg;
121  tf2::convert(transform, msg.transform);
122  msg.child_frame_id = "/map_"+std::to_string(num_submaps_);
123  msg.header.frame_id = "/map";
124  msg.header.stamp = ros::Time::now();
125  tfB_->sendTransform(msg);
126 
128  tf2::Transform(tf2::Quaternion(0.,0.,0.,1.0),
129  tf2::Vector3(0,0,0)); //no initial correction -- identity mat
131  Eigen::Vector3d(transform.getOrigin().getX(),
132  transform.getOrigin().getY(),0.);
133 
134  // create an interactive marker for the base of this frame and attach it
135  visualization_msgs::Marker m =
136  vis_utils::toMarker("map", "merge_maps_tool", 2.0);
137  m.pose.position.x = transform.getOrigin().getX();
138  m.pose.position.y = transform.getOrigin().getY();
139  m.id = num_submaps_;
140 
141  visualization_msgs::InteractiveMarker int_marker =
143  interactive_server_->insert(int_marker,
144  boost::bind(&MergeMapsKinematic::processInteractiveFeedback, this, _1));
145  interactive_server_->applyChanges();
146 
147  ROS_INFO("Map %s was loaded into topic %s!", req.filename.c_str(),
148  ("/map_"+std::to_string(num_submaps_)).c_str());
149  return true;
150 }
151 
152 /*****************************************************************************/
154  karto::Pose2& pose,
155  const tf2::Transform& submap_correction)
156 /*****************************************************************************/
157 {
158  tf2::Transform pose_tf, pose_corr;
159  tf2::Quaternion q(0.,0.,0.,1.0);
160  q.setRPY(0., 0., pose.GetHeading());
161  pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.));
162  pose_tf.setRotation(q);
163  pose_corr = submap_correction * pose_tf;
164  return karto::Pose2(pose_corr.getOrigin().x(), pose_corr.getOrigin().y(),
165  tf2::getYaw(pose_corr.getRotation()));
166 }
167 
168 /*****************************************************************************/
171  const tf2::Transform& submap_correction)
172 /*****************************************************************************/
173 {
174  tf2::Transform pose_tf, pose_corr;
175  pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.));
176  pose_tf.setRotation(tf2::Quaternion(0.,0.,0.,1.0));
177  pose_corr = submap_correction * pose_tf;
178  return karto::Vector2<kt_double>(pose_corr.getOrigin().x(),
179  pose_corr.getOrigin().y());
180 }
181 
182 /*****************************************************************************/
184  tf2::Transform& submap_correction)
185 /*****************************************************************************/
186 {
187  // TRANSFORM BARYCENTERR POSE
188  const karto::Pose2 bary_center_pose = (*iter)->GetBarycenterPose();
189  auto bary_center_pose_corr =
190  applyCorrection(bary_center_pose, submap_correction);
191  (*iter)->SetBarycenterPose(bary_center_pose_corr);
192 
193  // TRANSFORM BOUNDING BOX POSITIONS
194  karto::BoundingBox2 bbox = (*iter)->GetBoundingBox();
195  const karto::Vector2<kt_double> bbox_min_corr =
196  applyCorrection(bbox.GetMinimum(), submap_correction);
197  bbox.SetMinimum(bbox_min_corr);
198  const karto::Vector2<kt_double> bbox_max_corr =
199  applyCorrection(bbox.GetMaximum(), submap_correction);
200  bbox.SetMaximum(bbox_max_corr);
201  (*iter)->SetBoundingBox(bbox);
202 
203  // TRANSFORM UNFILTERED POINTS USED
204  karto::PointVectorDouble UPR_vec = (*iter)->GetPointReadings();
205  for(karto::PointVectorDouble::iterator it_upr = UPR_vec.begin();
206  it_upr != UPR_vec.end(); ++it_upr)
207  {
209  *it_upr, submap_correction);
210  it_upr->SetX(upr_corr.GetX());
211  it_upr->SetY(upr_corr.GetY());
212  }
213  (*iter)->SetPointReadings(UPR_vec);
214 
215  // TRANSFORM CORRECTED POSE
216  const karto::Pose2 corrected_pose = (*iter)->GetCorrectedPose();
217  karto::Pose2 karto_robot_pose_corr = applyCorrection(
218  corrected_pose, submap_correction);
219  (*iter)->SetCorrectedPose(karto_robot_pose_corr);
220  kt_bool dirty = false;
221  (*iter)->SetIsDirty(dirty);
222 
223  // TRANSFORM ODOM POSE
224  karto::Pose2 odom_pose = (*iter)->GetOdometricPose();
225  karto::Pose2 karto_robot_pose_odom = applyCorrection(
226  odom_pose, submap_correction);
227  (*iter)->SetOdometricPose(karto_robot_pose_odom);
228 }
229 
230 /*****************************************************************************/
232  slam_toolbox_msgs::MergeMaps::Request& req,
233  slam_toolbox_msgs::MergeMaps::Response& resp)
234 /*****************************************************************************/
235 {
236  ROS_INFO("Merging maps!");
237 
238  // transform all the scans into the new global map coordinates
239  int id = 0;
240  karto::LocalizedRangeScanVector transformed_scans;
241  for(LocalizedRangeScansVecIt it_LRV = scans_vec_.begin();
242  it_LRV != scans_vec_.end(); ++it_LRV)
243  {
244  id++;
245  for (LocalizedRangeScansIt iter = it_LRV->begin();
246  iter != it_LRV->end(); ++iter)
247  {
248  tf2::Transform submap_correction = submap_marker_transform_[id];
249  transformScan(iter, submap_correction);
250  transformed_scans.push_back((*iter));
251  }
252  }
253 
254  // create the map
255  nav_msgs::GetMap::Response map;
256  try
257  {
258  kartoToROSOccupancyGrid(transformed_scans, map);
259  } catch (const karto::Exception& e)
260  {
261  ROS_WARN("Failed to build grid to merge maps together, Exception: %s",
262  e.GetErrorMessage().c_str());
263  }
264 
265  // publish
266  map.map.header.stamp = ros::Time::now();
267  map.map.header.frame_id = "map";
268  sstS_[0].publish(map.map);
269  sstmS_[0].publish(map.map.info);
270 }
271 
272 /*****************************************************************************/
274  const karto::LocalizedRangeScanVector& scans,
275  nav_msgs::GetMap::Response& map)
276 /*****************************************************************************/
277 {
278  karto::OccupancyGrid* occ_grid = NULL;
280  if (!occ_grid)
281  {
282  ROS_INFO("MergeMapsKinematic: Could not make Karto occupancy grid.");
283  }
284  else
285  {
286  map.map.info.resolution = resolution_;
287  vis_utils::toNavMap(occ_grid, map.map);
288  }
289 
290  delete occ_grid;
291  return;
292 }
293 
294 /*****************************************************************************/
296  visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
297 /*****************************************************************************/
298 {
299  const int id = std::stoi(feedback->marker_name,nullptr,10);
300 
301  if (feedback->event_type ==
302  visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP &&
303  feedback->mouse_point_valid)
304  {
305  tfScalar yaw = tf2::getYaw(feedback->pose.orientation);
306  tf2::Quaternion quat(0.,0.,0.,1.0);
307  tf2::convert(feedback->pose.orientation, quat); // relative
308 
309  tf2::Transform previous_submap_correction;
310  previous_submap_correction.setIdentity();
311  previous_submap_correction.setOrigin(tf2::Vector3(submap_locations_[id](0),
312  submap_locations_[id](1), 0.));
313 
314  // update internal knowledge of submap locations
315  submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x,
316  feedback->pose.position.y,
317  submap_locations_[id](2) + yaw);
318 
319  // add the map_N frame there
320  tf2::Transform new_submap_location;
321  new_submap_location.setOrigin(tf2::Vector3(submap_locations_[id](0),
322  submap_locations_[id](1), 0.));
323  new_submap_location.setRotation(quat);
324 
325  geometry_msgs::TransformStamped msg;
326  tf2::convert(new_submap_location, msg.transform);
327  msg.child_frame_id = "/map_"+std::to_string(id);
328  msg.header.frame_id = "/map";
329  msg.header.stamp = ros::Time::now();
330  tfB_->sendTransform(msg);
331 
333  previous_submap_correction.inverse() * new_submap_location;
334  }
335 
336  if (feedback->event_type ==
337  visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
338  {
339  tfScalar yaw = tf2::getYaw(feedback->pose.orientation);
340  tf2::Quaternion quat(0.,0.,0.,1.0);
341  tf2::convert(feedback->pose.orientation, quat); // relative
342 
343  // add the map_N frame there
344  tf2::Transform new_submap_location;
345  new_submap_location.setOrigin(tf2::Vector3(feedback->pose.position.x,
346  feedback->pose.position.y, 0.));
347  new_submap_location.setRotation(quat);
348 
349  geometry_msgs::TransformStamped msg;
350  tf2::convert(new_submap_location, msg.transform);
351  msg.child_frame_id = "/map_"+std::to_string(id);
352  msg.header.frame_id = "/map";
353  msg.header.stamp = ros::Time::now();
354  tfB_->sendTransform(msg);
355  }
356 }
357 
358 /*****************************************************************************/
359 int main(int argc, char** argv)
360 /*****************************************************************************/
361 {
362  ros::init(argc, argv, "merge_maps_kinematic");
363  MergeMapsKinematic mmk;
364  ros::spin();
365  return 0;
366 }
#define NULL
void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
ros::ServiceServer ssSubmap_
void SetMinimum(const Vector2< kt_double > &mMinimum)
Definition: Karto.h:2891
int main(int argc, char **argv)
std::vector< karto::LocalizedRangeScanVector >::iterator LocalizedRangeScansVecIt
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1281
double tfScalar
std::map< int, tf2::Transform > submap_marker_transform_
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
std::unique_ptr< tf2_ros::TransformBroadcaster > tfB_
void transformScan(LocalizedRangeScansIt iter, tf2::Transform &submap_correction)
karto::Pose2 applyCorrection(const karto::Pose2 &pose, const tf2::Transform &submap_correction)
void kartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector &scans, nav_msgs::GetMap::Response &map)
void toNavMap(const karto::OccupancyGrid *occ_grid, nav_msgs::OccupancyGrid &map)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool read(const std::string &filename, karto::Mapper &mapper, karto::Dataset &dataset)
#define ROS_WARN(...)
const std::string & GetName() const
Definition: Karto.h:425
bool addSubmapCallback(slam_toolbox_msgs::AddSubmap::Request &req, slam_toolbox_msgs::AddSubmap::Response &resp)
void setIdentity()
Transform inverse() const
std::map< std::string, laser_utils::LaserMetadata > lasers_
std::vector< ros::Publisher > sstmS_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
#define ROS_INFO(...)
kt_double GetX() const
Definition: Karto.h:2097
karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
const T & GetX() const
Definition: Karto.h:1026
ros::ServiceServer ssMap_
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
Definition: Karto.h:6038
double getYaw(const A &a)
bool kt_bool
Definition: Types.h:64
visualization_msgs::Marker toMarker(const std::string &frame, const std::string &ns, const double &scale)
const Name & GetName() const
Definition: Karto.h:656
std::vector< std::unique_ptr< karto::Dataset > > dataset_vec_
Quaternion getRotation() const
visualization_msgs::InteractiveMarker toInteractiveMarker(visualization_msgs::Marker &marker, const double &scale)
static Time now()
void convert(const A &a, B &b)
const T & GetY() const
Definition: Karto.h:1044
kt_double GetY() const
Definition: Karto.h:2115
const Vector2< kt_double > & GetMinimum() const
Definition: Karto.h:2883
void SetMaximum(const Vector2< kt_double > &rMaximum)
Definition: Karto.h:2907
std::vector< karto::LocalizedRangeScanVector > scans_vec_
const Vector2< kt_double > & GetMaximum() const
Definition: Karto.h:2899
const std::string & GetErrorMessage() const
Definition: Karto.h:144
#define ROS_ERROR(...)
std::unique_ptr< interactive_markers::InteractiveMarkerServer > interactive_server_
bool mergeMapCallback(slam_toolbox_msgs::MergeMaps::Request &req, slam_toolbox_msgs::MergeMaps::Response &resp)
std::map< int, Eigen::Vector3d > submap_locations_
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5856
std::vector< ros::Publisher > sstS_
kt_double GetHeading() const
Definition: Karto.h:2151


slam_toolbox
Author(s): Steve Macenski
autogenerated on Wed Mar 3 2021 03:49:20