service to anchor missions manually

applications/maplab-server-node/include/maplab-server-node/maplab-server-node.h

@@ -101,6 +101,10 @@ class MaplabServerNode final {
     return total_num_merged_submaps_;
   }
 
+  bool anchorMissionManually(
+      const std::string& partial_mission_id_string,
+      const aslam::Transformation& T_G_M, std::string* status_message);
+
   void visualizeMap();
 
   void registerPoseCorrectionPublisherCallback(
@@ -131,6 +135,10 @@ class MaplabServerNode final {
 
   void saveMapEveryInterval();
 
+  bool resolvePartialMissionId(
+      const std::string& partial_mission_id_string,
+      vi_map::MissionId* mission_id, std::string* status_message) const;
+
   void runOneIterationOfMapMergingAlgorithms();
 
   void publishDenseMap();

applications/maplab-server-node/include/maplab-server-node/maplab-server-ros-node.h

@@ -11,6 +11,7 @@
 #include <maplab_msgs/DeleteAllRobotMissions.h>
 #include <maplab_msgs/DeleteMission.h>
 #include <maplab_msgs/GetDenseMapInRange.h>
+#include <maplab_msgs/AnchorMission.h>
 #include <ros/ros.h>
 #include <std_msgs/String.h>
 #include <std_srvs/Empty.h>
@@ -59,6 +60,10 @@ class MaplabServerRosNode {
       maplab_msgs::GetDenseMapInRange::Request& request,     // NOLINT
       maplab_msgs::GetDenseMapInRange::Response& response);  // NOLINT
 
+  bool anchorMissionServiceCallback(
+      maplab_msgs::AnchorMission::Request& request,     // NOLINT
+      maplab_msgs::AnchorMission::Response& response);  // NOLINT
+
   bool publishPoseCorrection(
       const int64_t timestamp_ns, const std::string& robot_name,
       const aslam::Transformation& T_G_curr_B_curr,
@@ -78,6 +83,7 @@ class MaplabServerRosNode {
   ros::ServiceServer delete_mission_srv_;
   ros::ServiceServer delete_all_robot_missions_srv_;
   ros::ServiceServer get_dense_map_in_range_srv_;
+  ros::ServiceServer anchor_mission_srv_;
 
   // State for running for maplab.
   ros::AsyncSpinner maplab_spinner_;

applications/maplab-server-node/src/maplab-server-node.cc

@@ -1388,8 +1388,9 @@ void MaplabServerNode::publishDenseMap() {
       robot_to_mission_id_map, *map);
 }
 
-bool MaplabServerNode::deleteMission(
-    const std::string& partial_mission_id_string, std::string* status_message) {
+bool MaplabServerNode::resolvePartialMissionId(
+    const std::string& partial_mission_id_string, vi_map::MissionId* mission_id,
+    std::string* status_message) const {
   CHECK_NOTNULL(status_message);
 
   std::lock_guard<std::mutex> lock(mutex_);
@@ -1411,15 +1412,14 @@ bool MaplabServerNode::deleteMission(
 
   // Retrieve full mission id.
   uint32_t num_matching_missions = 0u;
-  vi_map::MissionId mission_to_delete;
   std::string robot_name_of_mission;
   {
     std::lock_guard<std::mutex> lock(robot_to_mission_id_map_mutex_);
     for (const auto& kv : mission_id_to_robot_map_) {
       const std::string mission_id_string = kv.first.hexString();
       if (mission_id_string.substr(0, partial_mission_id_string_size) ==
           partial_mission_id_string) {
-        mission_to_delete = kv.first;
+        *mission_id = kv.first;
         robot_name_of_mission = kv.second;
         ++num_matching_missions;
       }
@@ -1443,11 +1443,27 @@ bool MaplabServerNode::deleteMission(
     return false;
   }
   CHECK_EQ(num_matching_missions, 1u);
-  CHECK(mission_to_delete.isValid());
+  CHECK(mission_id->isValid());
+  return true;
+}
 
+bool MaplabServerNode::deleteMission(
+    const std::string& partial_mission_id_string, std::string* status_message) {
+  vi_map::MissionId mission_to_delete;
+  const bool success = resolvePartialMissionId(
+      partial_mission_id_string, &mission_to_delete, status_message);
+  if (!success) {
+    return false;
+  }
   // Blacklist the mission, this will delete it at the end of the merging
   // iteration and prevent subsequent submap updates of this mission from
   // being merged.
+  std::string robot_name_of_mission;
+  std::stringstream ss;
+  {
+    std::lock_guard<std::mutex> lock(robot_to_mission_id_map_mutex_);
+    robot_name_of_mission = mission_id_to_robot_map_.at(mission_to_delete);
+  }
   {
     std::lock_guard<std::mutex> lock(blacklisted_missions_mutex_);
     blacklisted_missions_[mission_to_delete] = robot_name_of_mission;
@@ -1459,6 +1475,32 @@ bool MaplabServerNode::deleteMission(
   }
 }
 
+bool MaplabServerNode::anchorMissionManually(
+    const std::string& partial_mission_id_string,
+    const aslam::Transformation& T_G_M, std::string* status_message) {
+  // first check if merged map is available
+  if (!map_manager_.hasMap(kMergedMapKey)) {
+    *status_message = "Merged map is not available.";
+    LOG(ERROR) << "[MaplabServerNode] manual mission achoring: "
+               << *status_message;
+    return false;
+  }
+
+  vi_map::MissionId mission_to_anchor;
+  const bool success = resolvePartialMissionId(
+      partial_mission_id_string, &mission_to_anchor, status_message);
+  if (!success) {
+    return false;
+  }
+  vi_map::VIMapManager::MapWriteAccess map =
+      map_manager_.getMapWriteAccess(kMergedMapKey);
+
+  // vi_map::VIMission& mission = map->getMission(mission_to_anchor);
+  map->getMissionBaseFrameForMission(mission_to_anchor).set_T_G_M(T_G_M);
+  map->getMissionBaseFrameForMission(mission_to_anchor).set_is_T_G_M_known(true);
+  return true;
+}
+
 bool MaplabServerNode::deleteAllRobotMissions(
     const std::string& robot_name, std::string* status_message) {
   CHECK_NOTNULL(status_message);

applications/maplab-server-node/src/maplab-server-ros-node.cc

@@ -78,6 +78,14 @@ MaplabServerRosNode::MaplabServerRosNode(
   delete_all_robot_missions_srv_ = nh_.advertiseService(
       "delete_all_robot_missions", delete_all_robot_missions_callback);
 
+  boost::function<bool(
+      maplab_msgs::AnchorMission::Request&,
+      maplab_msgs::AnchorMission::Response&)>
+      anchor_mission_service_callback = boost::bind(
+          &MaplabServerRosNode::anchorMissionServiceCallback, this, _1, _2);
+  anchor_mission_srv_ =
+      nh_.advertiseService("anchor_mission", anchor_mission_service_callback);
+
   boost::function<bool(
       maplab_msgs::GetDenseMapInRange::Request&,
       maplab_msgs::GetDenseMapInRange::Response&)>
@@ -297,6 +305,21 @@ bool MaplabServerRosNode::deleteAllRobotMissionsCallback(
   return true;
 }
 
+bool MaplabServerRosNode::anchorMissionServiceCallback(
+    maplab_msgs::AnchorMission::Request& request,      // NOLINT
+    maplab_msgs::AnchorMission::Response& response) {  // NOLINT
+
+  const std::string& partial_mission_id_string =
+      request.mission_id_to_anchor.data;
+  const aslam::Transformation T_G_M_anchor;
+  tf::poseMsgToKindr(request.T_G_M, &T_G_M_anchor);
+
+  response.success.data = maplab_server_node_->anchorMissionManually(
+      partial_mission_id_string, T_G_M_anchor, &response.message.data);
+
+  return true;
+}
+
 bool MaplabServerRosNode::getDenseMapInRangeCallback(
     maplab_msgs::GetDenseMapInRange::Request& request,      // NOLINT
     maplab_msgs::GetDenseMapInRange::Response& response) {  // NOLINT