/*
 * slam_toolbox
 * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc.
 * Copyright Work Modifications (c) 2019, Samsung Research America
 *
 * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
 * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
 * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
 * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
 *
 * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
 * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
 * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
 * CONDITIONS.
 *
 */

 /* Orginal Author for slam_karto the original work was based on: Brian Gerkey */
 /* Author: Steven Macenski */

#include "slam_toolbox/slam_toolbox_common.hpp"
#include "slam_toolbox/serialization.hpp"

namespace slam_toolbox
{

  /*****************************************************************************/
  SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
    : solver_loader_("slam_toolbox", "karto::ScanSolver"),
    processor_type_(PROCESS),
    first_measurement_(true),
    nh_(nh),
    process_near_pose_(nullptr),
    using_loop_(false),
    initialized_(false)
    /*****************************************************************************/
  {
    if (!initialized_)
    {
      smapper_ = std::make_unique<mapper_utils::SMapper>();
      dataset_ = std::make_unique<karto::Dataset>();

      setParams(nh_);
      setROSInterfaces(nh_);
      setSolver(nh_);

      laser_assistant_ = std::make_unique<laser_utils::LaserAssistant>(
        nh_, tf_.get(), base_frame_);
      pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
        tf_.get(), base_frame_, odom_frame_);
      scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
      map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
      closure_assistant_ =
        std::make_unique<loop_closure_assistant::LoopClosureAssistant>(
          nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_);

      reprocessing_transform_.setIdentity();

      double transform_publish_period;
      nh_.param("transform_publish_period", transform_publish_period, 0.05);
      using_loop_ = true;
      threads_.push_back(std::make_unique<boost::thread>(
        boost::bind(&SlamToolbox::publishTransformLoop,
          this, transform_publish_period)));
      threads_.push_back(std::make_unique<boost::thread>(
        boost::bind(&SlamToolbox::publishVisualizations, this)));

      bool all_joinable = false;
      while (ros::ok() && !all_joinable)
      {
        all_joinable = true;
        for (auto& p : threads_)
        {
          if (!p->joinable())
          {
            all_joinable = false;
            break;
          }
        }
      }

      ROS_INFO("All threads are joinable, proceeding with next steps.");
      initialized_ = true;
    }
  }

  /*****************************************************************************/
  SlamToolbox::~SlamToolbox()
    /*****************************************************************************/
  {
    using_loop_ = false;
    for (int i = 0; i != threads_.size(); i++)
    {
      threads_[i]->join();
    }

    smapper_.reset();
    dataset_.reset();
    closure_assistant_.reset();
    map_saver_.reset();
    pose_helper_.reset();
    laser_assistant_.reset();
    scan_holder_.reset();
  }

  /*****************************************************************************/
  void SlamToolbox::setSolver(ros::NodeHandle& private_nh_)
    /*****************************************************************************/
  {
    // Set solver to be used in loop closure
    std::string solver_plugin;
    if (!private_nh_.getParam("solver_plugin", solver_plugin))
    {
      ROS_WARN("unable to find requested solver plugin, defaulting to SPA");
      solver_plugin = "solver_plugins::CeresSolver";
    }
    try
    {
      solver_ = solver_loader_.createInstance(solver_plugin);
      ROS_INFO("Using plugin %s", solver_plugin.c_str());
    }
    catch (const pluginlib::PluginlibException& ex)
    {
      ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.",
        solver_plugin.c_str(), ex.what());
      exit(1);
    }
    smapper_->getMapper()->SetScanSolver(solver_.get());
  }

  /*****************************************************************************/
  void SlamToolbox::setParams(ros::NodeHandle& private_nh)
  /*****************************************************************************/
  {
    map_to_odom_.setIdentity();
    private_nh.param("odom_frame", odom_frame_, std::string("odom"));
    private_nh.param("map_frame", map_frame_, std::string("map"));
    private_nh.param("base_frame", base_frame_, std::string("base_footprint"));
    private_nh.param("resolution", resolution_, 0.05);
    private_nh.param("map_name", map_name_, std::string("/map"));
    private_nh.param("scan_topic", scan_topic_, std::string("/scan"));
    private_nh.param("throttle_scans", throttle_scans_, 1);
    private_nh.param("enable_interactive_mode", enable_interactive_mode_, false);

    double tmp_val;
    private_nh.param("transform_timeout", tmp_val, 0.2);
    transform_timeout_ = ros::Duration(tmp_val);
    private_nh.param("tf_buffer_duration", tmp_val, 30.);
    tf_buffer_dur_ = ros::Duration(tmp_val);
    private_nh.param("minimum_time_interval", tmp_val, 0.5);
    minimum_time_interval_ = ros::Duration(tmp_val);

    private_nh.param("position_covariance_scale", position_covariance_scale_, 1.0);
    private_nh.param("yaw_covariance_scale", yaw_covariance_scale_, 1.0);

    bool debug = false;
    if (private_nh.getParam("debug_logging", debug) && debug)
    {
      if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
        ros::console::levels::Debug))
      {
        ros::console::notifyLoggerLevelsChanged();
      }
    }

    smapper_->configure(private_nh);
    private_nh.setParam("paused_new_measurements", false);
  }

  /*****************************************************************************/
  void SlamToolbox::setROSInterfaces(ros::NodeHandle& node)
    /*****************************************************************************/
  {
    tf_ = std::make_unique<tf2_ros::Buffer>(ros::Duration(tf_buffer_dur_));
    tfL_ = std::make_unique<tf2_ros::TransformListener>(*tf_);
    tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
    sst_ = node.advertise<nav_msgs::OccupancyGrid>(map_name_, 1, true);
    sstm_ = node.advertise<nav_msgs::MapMetaData>(map_name_ + "_metadata", 1, true);
    ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this);
    ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this);
    ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this);
    ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this);
    ssReset_ = node.advertiseService("reset", &SlamToolbox::resetCallback, this);
    scan_filter_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, scan_topic_, 5);
    scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_sub_, *tf_, odom_frame_, 5, node);
    scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
    pose_pub_ = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 10, true);
  }

  /*****************************************************************************/
  void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
    /*****************************************************************************/
  {
    if (transform_publish_period == 0)
    {
      return;
    }

    ros::Rate r(1.0 / transform_publish_period);
    while (ros::ok() && using_loop_)
    {
      {
        boost::mutex::scoped_lock lock(map_to_odom_mutex_);
        geometry_msgs::TransformStamped msg;
        tf2::convert(map_to_odom_, msg.transform);
        msg.child_frame_id = odom_frame_;
        msg.header.frame_id = map_frame_;

        msg.header.stamp = ros::Time::now() + transform_timeout_;
        tfB_->sendTransform(msg);
      }
      r.sleep();
    }
  }

  /*****************************************************************************/
  void SlamToolbox::publishVisualizations()
    /*****************************************************************************/
  {
    nav_msgs::OccupancyGrid& og = map_.map;
    og.info.resolution = resolution_;
    og.info.origin.position.x = 0.0;
    og.info.origin.position.y = 0.0;
    og.info.origin.position.z = 0.0;
    og.info.origin.orientation.x = 0.0;
    og.info.origin.orientation.y = 0.0;
    og.info.origin.orientation.z = 0.0;
    og.info.origin.orientation.w = 1.0;
    og.header.frame_id = map_frame_;

    double map_update_interval;
    if (!nh_.getParam("map_update_interval", map_update_interval))
      map_update_interval = 10.0;
    ros::Rate r(1.0 / map_update_interval);
    while (ros::ok() && using_loop_)
    {
      this->updateMap();
      if (!isPaused(VISUALIZING_GRAPH))
      {
        boost::mutex::scoped_lock lock(smapper_mutex_);
        closure_assistant_->publishGraph();
      }
      r.sleep();
    }
  }

  /*****************************************************************************/
  void SlamToolbox::loadPoseGraphByParams(ros::NodeHandle& nh)
    /*****************************************************************************/
  {
    std::string filename;
    geometry_msgs::Pose2D pose;
    bool dock = false;
    if (shouldStartWithPoseGraph(filename, pose, dock))
    {
      slam_toolbox_msgs::DeserializePoseGraph::Request req;
      slam_toolbox_msgs::DeserializePoseGraph::Response resp;
      req.initial_pose = pose;
      req.filename = filename;
      if (dock)
      {
        req.match_type =
          slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE;
      }
      else
      {
        req.match_type =
          slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE;
      }
      deserializePoseGraphCallback(req, resp);
    }
  }

  /*****************************************************************************/
  bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename,
    geometry_msgs::Pose2D& pose, bool& start_at_dock)
    /*****************************************************************************/
  {
    // if given a map to load at run time, do it.
    if (nh_.getParam("map_file_name", filename))
    {
      std::vector<double> read_pose;
      if (nh_.getParam("map_start_pose", read_pose))
      {
        start_at_dock = false;
        if (read_pose.size() != 3)
        {
          ROS_ERROR("LocalizationSlamToolbox: Incorrect number of "
            "arguments for map starting pose. Must be in format: "
            "[x, y, theta]. Starting at the origin");
          pose.x = 0.;
          pose.y = 0.;
          pose.theta = 0.;
        }
        else
        {
          pose.x = read_pose[0];
          pose.y = read_pose[1];
          pose.theta = read_pose[2];
        }
      }
      else
      {
        nh_.getParam("map_start_at_dock", start_at_dock);
      }

      return true;
    }

    return false;
  }

  /*****************************************************************************/
  karto::LaserRangeFinder* SlamToolbox::getLaser(const
    sensor_msgs::LaserScan::ConstPtr& scan)
    /*****************************************************************************/
  {
    const std::string& frame = scan->header.frame_id;
    if (lasers_.find(frame) == lasers_.end())
    {
      try
      {
        lasers_[frame] = laser_assistant_->toLaserMetadata(*scan);
        dataset_->Add(lasers_[frame].getLaser(), true);
      }
      catch (tf2::TransformException& e)
      {
        ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)",
          e.what());
        return nullptr;
      }
    }

    return lasers_[frame].getLaser();
  }

  /*****************************************************************************/
  bool SlamToolbox::updateMap()
    /*****************************************************************************/
  {
    if (sst_.getNumSubscribers() == 0)
    {
      return true;
    }
    boost::mutex::scoped_lock lock(smapper_mutex_);
    karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_);
    if (!occ_grid)
    {
      return false;
    }

    vis_utils::toNavMap(occ_grid, map_.map);

    // publish map as current
    map_.map.header.stamp = ros::Time::now();
    sst_.publish(map_.map);
    sstm_.publish(map_.map.info);

    delete occ_grid;
    occ_grid = nullptr;
    return true;
  }

  /*****************************************************************************/
  tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
    const karto::Pose2& corrected_pose,
    const karto::Pose2& karto_pose,
    const ros::Time& t,
    const bool& update_reprocessing_transform)
    /*****************************************************************************/
  {
    // Compute the map->odom transform
    tf2::Stamped<tf2::Transform> odom_to_map;
    tf2::Quaternion q(0., 0., 0., 1.0);
    q.setRPY(0., 0., corrected_pose.GetHeading());
    tf2::Stamped<tf2::Transform> base_to_map(
      tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(),
        corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_);
    try
    {
      geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg;
      tf2::convert(base_to_map, base_to_map_msg);
      odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_);
      tf2::convert(odom_to_map_msg, odom_to_map);
    }
    catch (tf2::TransformException& e)
    {
      ROS_ERROR("Transform from base_link to odom failed: %s", e.what());
      return odom_to_map;
    }

    // if we're continuing a previous session, we need to
    // estimate the homogenous transformation between the old and new
    // odometry frames and transform the new session 
    // into the older session's frame
    if (update_reprocessing_transform)
    {
      tf2::Transform odom_to_base_serialized = base_to_map.inverse();
      tf2::Quaternion q1(0., 0., 0., 1.0);
      q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation()));
      odom_to_base_serialized.setRotation(q1);
      tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose);
      reprocessing_transform_ =
        odom_to_base_serialized * odom_to_base_current.inverse();
    }

    // set map to odom for our transformation thread to publish
    boost::mutex::scoped_lock lock(map_to_odom_mutex_);
    map_to_odom_ = tf2::Transform(tf2::Quaternion(odom_to_map.getRotation()),
      tf2::Vector3(odom_to_map.getOrigin())).inverse();

    return odom_to_map;
  }

  /*****************************************************************************/
  karto::LocalizedRangeScan* SlamToolbox::getLocalizedRangeScan(
    karto::LaserRangeFinder* laser,
    const sensor_msgs::LaserScan::ConstPtr& scan,
    karto::Pose2& karto_pose)
    /*****************************************************************************/
  {
    // Create a vector of doubles for karto
    std::vector<kt_double> readings = laser_utils::scanToReadings(
      *scan, lasers_[scan->header.frame_id].isInverted());

    // transform by the reprocessing transform
    tf2::Transform pose_original = smapper_->toTfPose(karto_pose);
    tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original;
    karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed);

    // create localized range scan
    karto::LocalizedRangeScan* range_scan = new karto::LocalizedRangeScan(
      laser->GetName(), readings);
    range_scan->SetOdometricPose(transformed_pose);
    range_scan->SetCorrectedPose(transformed_pose);
    return range_scan;
  }

  /*****************************************************************************/
  bool SlamToolbox::shouldProcessScan(
    const sensor_msgs::LaserScan::ConstPtr& scan,
    const karto::Pose2& pose)
    /*****************************************************************************/
  {
    static karto::Pose2 last_pose;
    static ros::Time last_scan_time = ros::Time(0.);
    static double min_dist2 =
      smapper_->getMapper()->getParamMinimumTravelDistance() *
      smapper_->getMapper()->getParamMinimumTravelDistance();

    // we give it a pass on the first measurement to get the ball rolling
    if (first_measurement_)
    {
      last_scan_time = scan->header.stamp;
      last_pose = pose;
      first_measurement_ = false;
      return true;
    }

    // we are in a paused mode, reject incomming information
    if (isPaused(NEW_MEASUREMENTS))
    {
      return false;
    }

    // throttled out
    if ((scan->header.seq % throttle_scans_) != 0)
    {
      return false;
    }

    // not enough time
    if (scan->header.stamp - last_scan_time < minimum_time_interval_)
    {
      return false;
    }

    // check moved enough, within 10% for correction error
    const double dist2 = last_pose.SquaredDistance(pose);
    if (dist2 < 0.8 * min_dist2 || scan->header.seq < 5)
    {
      return false;
    }

    last_pose = pose;
    last_scan_time = scan->header.stamp;

    return true;
  }

  /*****************************************************************************/
  karto::LocalizedRangeScan* SlamToolbox::addScan(
    karto::LaserRangeFinder* laser,
    PosedScan& scan_w_pose)
    /*****************************************************************************/
  {
    return addScan(laser, scan_w_pose.scan, scan_w_pose.pose);
  }

  /*****************************************************************************/
  karto::LocalizedRangeScan* SlamToolbox::addScan(
    karto::LaserRangeFinder* laser,
    const sensor_msgs::LaserScan::ConstPtr& scan,
    karto::Pose2& karto_pose)
    /*****************************************************************************/
  {
    // get our localized range scan
    karto::LocalizedRangeScan* range_scan = getLocalizedRangeScan(
      laser, scan, karto_pose);

    // Add the localized range scan to the smapper
    boost::mutex::scoped_lock lock(smapper_mutex_);
    bool processed = false, update_reprocessing_transform = false;

    karto::Matrix3 covariance;
    covariance.SetToIdentity();

    if (processor_type_ == PROCESS)
    {
      processed = smapper_->getMapper()->Process(range_scan, &covariance);
    }
    else if (processor_type_ == PROCESS_FIRST_NODE)
    {
      processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance);
      processor_type_ = PROCESS;
      update_reprocessing_transform = true;
    }
    else if (processor_type_ == PROCESS_NEAR_REGION)
    {
      boost::mutex::scoped_lock l(pose_mutex_);
      if (!process_near_pose_)
      {
        ROS_ERROR("Process near region called without a "
          "valid region request. Ignoring scan.");
        return nullptr;
      }
      range_scan->SetOdometricPose(*process_near_pose_);
      range_scan->SetCorrectedPose(range_scan->GetOdometricPose());
      process_near_pose_.reset(nullptr);
      processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, false, &covariance);
      update_reprocessing_transform = true;
      processor_type_ = PROCESS;
    }
    else
    {
      ROS_FATAL("SlamToolbox: No valid processor type set! Exiting.");
      exit(-1);
    }

    // if successfully processed, create odom to map transformation
    // and add our scan to storage
    if (processed)
    {
      if (enable_interactive_mode_)
      {
        scan_holder_->addScan(*scan);
      }

      setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose,
        scan->header.stamp, update_reprocessing_transform);
      dataset_->Add(range_scan);
      publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp);
    }
    else
    {
      delete range_scan;
      range_scan = nullptr;
    }

    return range_scan;
  }

  /*****************************************************************************/
  void SlamToolbox::publishPose(
    const karto::Pose2& pose,
    const karto::Matrix3& cov,
    const ros::Time& t)
    /*****************************************************************************/
  {
    geometry_msgs::PoseWithCovarianceStamped pose_msg;
    pose_msg.header.stamp = t;
    pose_msg.header.frame_id = map_frame_;

    tf2::Quaternion q(0., 0., 0., 1.0);
    q.setRPY(0., 0., pose.GetHeading());
    tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0));
    tf2::toMsg(transform, pose_msg.pose.pose);

    pose_msg.pose.covariance[0] = cov(0, 0) * position_covariance_scale_;  // x
    pose_msg.pose.covariance[1] = cov(0, 1) * position_covariance_scale_;  // xy
    pose_msg.pose.covariance[6] = cov(1, 0) * position_covariance_scale_;  // xy
    pose_msg.pose.covariance[7] = cov(1, 1) * position_covariance_scale_;  // y
    pose_msg.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_;      // yaw

    pose_pub_.publish(pose_msg);
  }

  /*****************************************************************************/
  bool SlamToolbox::mapCallback(
    nav_msgs::GetMap::Request& req,
    nav_msgs::GetMap::Response& res)
    /*****************************************************************************/
  {
    if (map_.map.info.width && map_.map.info.height)
    {
      boost::mutex::scoped_lock lock(smapper_mutex_);
      res = map_;
      return true;
    }
    else
    {
      return false;
    }
  }

  /*****************************************************************************/
  bool SlamToolbox::pauseNewMeasurementsCallback(
    slam_toolbox_msgs::Pause::Request& req,
    slam_toolbox_msgs::Pause::Response& resp)
    /*****************************************************************************/
  {
    bool curr_state = isPaused(NEW_MEASUREMENTS);
    state_.set(NEW_MEASUREMENTS, !curr_state);

    nh_.setParam("paused_new_measurements", !curr_state);
    ROS_INFO("SlamToolbox: Toggled to %s",
      !curr_state ? "pause taking new measurements." :
      "actively taking new measurements.");
    resp.status = true;
    return true;
  }

  /*****************************************************************************/
  bool SlamToolbox::isPaused(const PausedApplication& app)
    /*****************************************************************************/
  {
    return state_.get(app);
  }

  /*****************************************************************************/
  bool SlamToolbox::serializePoseGraphCallback(
    slam_toolbox_msgs::SerializePoseGraph::Request& req,
    slam_toolbox_msgs::SerializePoseGraph::Response& resp)
    /*****************************************************************************/
  {
    std::string filename = req.filename;

    // if we're inside the snap, we need to write to commonly accessible space
    if (snap_utils::isInSnap())
    {
      filename = snap_utils::getSnapPath() + std::string("/") + filename;
    }

    boost::mutex::scoped_lock lock(smapper_mutex_);
    serialization::write(filename, *smapper_->getMapper(), *dataset_);
    return true;
  }

  /*****************************************************************************/
  void SlamToolbox::loadSerializedPoseGraph(
    std::unique_ptr<karto::Mapper>& mapper,
    std::unique_ptr<karto::Dataset>& dataset)
    /*****************************************************************************/
  {
    boost::mutex::scoped_lock lock(smapper_mutex_);

    solver_->Reset();

    // add the nodes and constraints to the optimizer
    VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices();
    VerticeMap::iterator vertex_map_it = mapper_vertices.begin();
    for (vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it)
    {
      ScanMap::iterator vertex_it = vertex_map_it->second.begin();
      for (vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it)
      {
        if (vertex_it->second != nullptr)
        {
          solver_->AddNode(vertex_it->second);
        }
      }
    }

    EdgeVector mapper_edges = mapper->GetGraph()->GetEdges();
    EdgeVector::iterator edges_it = mapper_edges.begin();
    for (edges_it; edges_it != mapper_edges.end(); ++edges_it)
    {
      if (*edges_it != nullptr)
      {
        solver_->AddConstraint(*edges_it);
      }
    }

    mapper->SetScanSolver(solver_.get());

    // move the memory to our working dataset
    smapper_->setMapper(mapper.release());
    smapper_->configure(nh_);
    dataset_.reset(dataset.release());

    closure_assistant_->setMapper(smapper_->getMapper());

    if (!smapper_->getMapper())
    {
      ROS_FATAL("loadSerializedPoseGraph: Could not properly load "
        "a valid mapping object. Did you modify something by hand?");
      exit(-1);
    }

    if (dataset_->GetLasers().size() < 1)
    {
      ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize "
        "dataset with no laser objects.");
      exit(-1);
    }

    // create a current laser sensor
    karto::LaserRangeFinder* laser =
      dynamic_cast<karto::LaserRangeFinder*>(
        dataset_->GetLasers()[0]);
    karto::Sensor* pSensor = dynamic_cast<karto::Sensor*>(laser);
    if (pSensor)
    {
      karto::SensorManager::GetInstance()->RegisterSensor(pSensor);

      while (ros::ok())
      {
        ROS_INFO("Waiting for incoming scan to get metadata...");
        boost::shared_ptr<sensor_msgs::LaserScan const> scan =
          ros::topic::waitForMessage<sensor_msgs::LaserScan>(
            scan_topic_, ros::Duration(1.0));
        if (scan)
        {
          ROS_INFO("Got scan!");
          try
          {
            lasers_[scan->header.frame_id] =
              laser_assistant_->toLaserMetadata(*scan);
            break;
          }
          catch (tf2::TransformException& e)
          {
            ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)",
              e.what());
            exit(-1);
          }
        }
      }
    }
    else
    {
      ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor.");
    }

    solver_->Compute();

    return;
  }

  /*****************************************************************************/
  bool SlamToolbox::deserializePoseGraphCallback(
    slam_toolbox_msgs::DeserializePoseGraph::Request& req,
    slam_toolbox_msgs::DeserializePoseGraph::Response& resp)
    /*****************************************************************************/
  {
    if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET)
    {
      ROS_ERROR("Deserialization called without valid processor type set. "
        "Undefined behavior!");
      return false;
    }

    std::string filename = req.filename;

    if (filename.empty())
    {
      ROS_WARN("No map file given!");
      return true;
    }

    // if we're inside the snap, we need to write to commonly accessible space
    if (snap_utils::isInSnap())
    {
      filename = snap_utils::getSnapPath() + std::string("/") + filename;
    }

    std::unique_ptr<karto::Dataset> dataset = std::make_unique<karto::Dataset>();
    std::unique_ptr<karto::Mapper> mapper = std::make_unique<karto::Mapper>();

    if (!serialization::read(filename, *mapper, *dataset))
    {
      ROS_ERROR("DeserializePoseGraph: Failed to read "
        "file: %s.", filename.c_str());
      return true;
    }
    ROS_DEBUG("DeserializePoseGraph: Successfully read file.");

    loadSerializedPoseGraph(mapper, dataset);
    updateMap();

    first_measurement_ = true;
    boost::mutex::scoped_lock l(pose_mutex_);
    switch (req.match_type)
    {
    case procType::START_AT_FIRST_NODE:
      processor_type_ = PROCESS_FIRST_NODE;
      break;
    case procType::START_AT_GIVEN_POSE:
      processor_type_ = PROCESS_NEAR_REGION;
      process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
        req.initial_pose.y, req.initial_pose.theta);
      break;
    case procType::LOCALIZE_AT_POSE:
      processor_type_ = PROCESS_LOCALIZATION;
      process_near_pose_ = std::make_unique<karto::Pose2>(req.initial_pose.x,
        req.initial_pose.y, req.initial_pose.theta);
      break;
    default:
      ROS_FATAL("Deserialization called without valid processor type set.");
    }

    return true;
  }

  /*****************************************************************************/
  bool SlamToolbox::resetCallback(
    slam_toolbox_msgs::Reset::Request& req,
    slam_toolbox_msgs::Reset::Response& resp)
    /*****************************************************************************/
  {
    boost::mutex::scoped_lock lock(smapper_mutex_);
    // Reset the map.
    smapper_->Reset();
    smapper_->getMapper()->getScanSolver()->Reset();

    // Ensure we will process the next available scan.
    first_measurement_ = true;

    // Pause new measurements processing if requested.
    if (req.pause_new_measurements) {
      state_.set(NEW_MEASUREMENTS, true);
      nh_.setParam("paused_new_measurements", true);
      ROS_INFO("SlamToolbox: Toggled to pause taking new measurements after reset.");
    }

    resp.result = true;
    return true;
  }

} // end namespace
