[Fix]map merge misalignment (SteveMacenski#704)

* fix map merge generate map misalignment

* fix rotation issue

Author: Zarqu0n

Diff for: include/slam_toolbox/merge_maps_kinematic.hpp

@@ -101,6 +101,7 @@ class MergeMapsKinematic : public rclcpp::Node
   std::map<int, Eigen::Vector3d> submap_locations_;
   std::vector<karto::LocalizedRangeScanVector> scans_vec_;
   std::map<int, tf2::Transform> submap_marker_transform_;
+  std::map<int, tf2::Transform> prev_submap_marker_transform_;
   double resolution_;
   int num_submaps_;
 };

Diff for: src/merge_maps_kinematic.cpp

@@ -136,6 +136,7 @@ bool MergeMapsKinematic::addSubmapCallback(
   submap_marker_transform_[num_submaps_] =
     tf2::Transform(tf2::Quaternion(0., 0., 0., 1.0),
       tf2::Vector3(0, 0, 0));  // no initial correction -- identity mat
+  prev_submap_marker_transform_ = submap_marker_transform_;
   submap_locations_[num_submaps_] =
     Eigen::Vector3d(transform.getOrigin().getX(),
       transform.getOrigin().getY(), 0.);
@@ -267,11 +268,12 @@ bool MergeMapsKinematic::mergeMapCallback(
     for (LocalizedRangeScansIt iter = it_LRV->begin();
       iter != it_LRV->end(); ++iter)
     {
-      tf2::Transform submap_correction = submap_marker_transform_[id];
+      tf2::Transform submap_correction = submap_marker_transform_[id] * prev_submap_marker_transform_[id].inverse();
       transformScan(iter, submap_correction);
       transformed_scans.push_back((*iter));
     }
   }
+  prev_submap_marker_transform_ = submap_marker_transform_;
 
   // create the map
   nav_msgs::srv::GetMap::Response map;
@@ -317,46 +319,47 @@ void MergeMapsKinematic::processInteractiveFeedback(
 {
   const int id = std::stoi(feedback->marker_name, nullptr, 10);
 
+  tf2::Quaternion quat(feedback->pose.orientation.x,
+    feedback->pose.orientation.y,
+    feedback->pose.orientation.z,
+    feedback->pose.orientation.w);
+  
   if (feedback->event_type ==
     visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP &&
     feedback->mouse_point_valid)
   {
     tf2Scalar yaw = tf2::getYaw(feedback->pose.orientation);
-    tf2::Quaternion quat(0., 0., 0., 1.0);
     tf2::fromMsg(feedback->pose.orientation, quat);  // relative
 
     tf2::Transform previous_submap_correction;
     previous_submap_correction.setIdentity();
     previous_submap_correction.setOrigin(tf2::Vector3(submap_locations_[id](0),
       submap_locations_[id](1), 0.));
-
+    previous_submap_correction.setRotation(submap_marker_transform_[id].getRotation());
     // update internal knowledge of submap locations
     submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x,
         feedback->pose.position.y,
-        submap_locations_[id](2) + yaw);
+        0.0);
 
     // add the map_N frame there
     tf2::Transform new_submap_location;
-    new_submap_location.setOrigin(tf2::Vector3(submap_locations_[id](0),
-      submap_locations_[id](1), 0.));
+    new_submap_location.setOrigin(tf2::Vector3(feedback->pose.position.x,
+      feedback->pose.position.y, 0.));
     new_submap_location.setRotation(quat);
-
     geometry_msgs::msg::TransformStamped msg;
     msg.transform = tf2::toMsg(new_submap_location);
     msg.child_frame_id = "/map_" + std::to_string(id);
     msg.header.frame_id = "/map";
     msg.header.stamp = this->now();
     tfB_->sendTransform(msg);
 
-    submap_marker_transform_[id] = submap_marker_transform_[id] *
-      previous_submap_correction.inverse() * new_submap_location;
+    submap_marker_transform_[id] = new_submap_location;
   }
 
   if (feedback->event_type ==
     visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE)
   {
     tf2Scalar yaw = tf2::getYaw(feedback->pose.orientation);
-    tf2::Quaternion quat(0., 0., 0., 1.0);
     tf2::fromMsg(feedback->pose.orientation, quat);  // relative
 
     // add the map_N frame there
@@ -372,6 +375,7 @@ void MergeMapsKinematic::processInteractiveFeedback(
     msg.header.stamp = this->now();
     tfB_->sendTransform(msg);
   }
+  this->mergeMapCallback(nullptr, nullptr, nullptr);
 }
 
 /*****************************************************************************/