text: /***********************************************************************************
   * Revised BSD License *
   * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at> *
   * All rights reserved. *
   *                                                                                 *
   * Redistribution and use in source and binary forms, with or without *
   * modification, are permitted provided that the following conditions are met: *
   *     * Redistributions of source code must retain the above copyright *
   *       notice, this list of conditions and the following disclaimer. *
   *     * Redistributions in binary form must reproduce the above copyright *
   *       notice, this list of conditions and the following disclaimer in the *
   *       documentation and/or other materials provided with the distribution. *
   *     * Neither the name of the Vienna University of Technology nor the *
   *       names of its contributors may be used to endorse or promote products *
   *       derived from this software without specific prior written permission. *
   *                                                                                 *
   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   *AND *
   * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
   **
   * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
   * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY *
   * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
   * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   **
   * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
   * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
   * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   **
   * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   **                       *
   ***********************************************************************************/
  
  #include <boost/interprocess/sync/scoped_lock.hpp>
  #include <geometry_msgs/PoseArray.h>
  #include <pose_cov_ops/pose_cov_ops.h>
  
  #include <mrpt_bridge/pose.h>
  #include <mrpt_bridge/laser_scan.h>
  #include <mrpt_bridge/time.h>
  #include <mrpt_bridge/map.h>
  #include <mrpt_bridge/beacon.h>
  
  #include <mrpt/version.h>
  #include <mrpt/obs/CObservationBeaconRanges.h>
  using namespace mrpt::obs;
  
  #include <mrpt/version.h>
  //
  #if MRPT_VERSION >= 0x199
  #include <mrpt/system/COutputLogger.h>
  using namespace mrpt::system;
  #else
  #include <mrpt/utils/COutputLogger.h>
  using namespace mrpt::utils;
  #endif
  
  #include <mrpt/obs/CObservationRobotPose.h>
  
  #include "mrpt_localization_node.h"
  
  #include <mrpt/maps/COccupancyGridMap2D.h>
  using mrpt::maps::COccupancyGridMap2D;
  
  int main(int argc, char** argv)
  {
  	ros::init(argc, argv, "localization");
  	ros::NodeHandle nh;
  	PFLocalizationNode my_node(nh);
  	my_node.init();
  	my_node.loop();
  	return 0;
  }
  
  PFLocalizationNode::~PFLocalizationNode() {}
  PFLocalizationNode::PFLocalizationNode(ros::NodeHandle& n)
  	: PFLocalization(new PFLocalizationNode::Parameters(this)),
  	  nh_(n),
  	  first_map_received_(false),
  	  loop_count_(0)
  {
  }
  
  PFLocalizationNode::Parameters* PFLocalizationNode::param()
  {
  	return (PFLocalizationNode::Parameters*)param_;
  }
  
  void PFLocalizationNode::init()
  {
  	// Use MRPT library the same log level as on ROS nodes (only for
  	// MRPT_VERSION >= 0x150)
  	useROSLogLevel();
  
  	PFLocalization::init();
  	sub_init_pose_ = nh_.subscribe(
  		"initialpose", 1, &PFLocalizationNode::callbackInitialpose, this);
  
  	sub_odometry_ =
  		nh_.subscribe("odom", 1, &PFLocalizationNode::callbackOdometry, this);
  
  	// Subscribe to one or more laser sources:
  	std::vector<std::string> sources;
  	mrpt::system::tokenize(param()->sensor_sources, " ,\t\n", sources);
  	ROS_ASSERT_MSG(
  		!sources.empty(),
  		"*Fatal*: At least one sensor source must be provided in "
  		"~sensor_sources (e.g. \"scan\" or \"beacon\")");
  	sub_sensors_.resize(sources.size());
  	for (size_t i = 0; i < sources.size(); i++)
  	{
  		if (sources[i].find("scan") != std::string::npos)
  		{
  			sub_sensors_[i] = nh_.subscribe(
  				sources[i], 1, &PFLocalizationNode::callbackLaser, this);
  		}
  		else if (sources[i].find("beacon") != std::string::npos)
  		{
  			sub_sensors_[i] = nh_.subscribe(
  				sources[i], 1, &PFLocalizationNode::callbackBeacon, this);
  		}
  		else
  		{
  			sub_sensors_[i] = nh_.subscribe(
  				sources[i], 1, &PFLocalizationNode::callbackRobotPose, this);
  		}
  	}
  
  	if (!param()->map_file.empty())
  	{
  #if MRPT_VERSION >= 0x199
  		if (metric_map_.countMapsByClass<COccupancyGridMap2D>())
  		{
  			mrpt_bridge::convert(
  				*metric_map_.mapByClass<COccupancyGridMap2D>(), resp_.map);
  		}
  #else
  		if (metric_map_.m_gridMaps.size())
  		{
  			mrpt_bridge::convert(*metric_map_.m_gridMaps[0], resp_.map);
  		}
  #endif
  		pub_map_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  		pub_metadata_ =
  			nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  		service_map_ = nh_.advertiseService(
  			"static_map", &PFLocalizationNode::mapCallback, this);
  	}
  	pub_particles_ =
  		nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 1, true);
  
  	pub_pose_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
  		"mrpt_pose", 2, true);
  }
  
  void PFLocalizationNode::loop()
  {
  	ROS_INFO("loop");
  	for (ros::Rate rate(param()->rate); ros::ok(); loop_count_++)
  	{
  		param()->update(loop_count_);
  
  		if ((loop_count_ % param()->map_update_skip == 0) &&
  #if MRPT_VERSION >= 0x199
  			(metric_map_.countMapsByClass<COccupancyGridMap2D>()))
  #else
  			(metric_map_.m_gridMaps.size()))
  #endif
  			publishMap();
  		if (loop_count_ % param()->particlecloud_update_skip == 0)
  			publishParticles();
  		if (param()->tf_broadcast) publishTF();
  		if (param()->pose_broadcast) publishPose();
  
  		ros::spinOnce();
  		rate.sleep();
  	}
  }
  
  bool PFLocalizationNode::waitForTransform(
  	mrpt::poses::CPose3D& des, const std::string& target_frame,
  	const std::string& source_frame, const ros::Time& time,
  	const ros::Duration& timeout, const ros::Duration& polling_sleep_duration)
  {
  	tf::StampedTransform transform;
  	try
  	{
  		tf_listener_.waitForTransform(
  			target_frame, source_frame, time, timeout, polling_sleep_duration);
  		tf_listener_.lookupTransform(
  			target_frame, source_frame, time, transform);
  	}
  	catch (tf::TransformException& e)
  	{
  		ROS_WARN(
  			"Failed to get transform target_frame (%s) to source_frame (%s): "
  			"%s",
  			target_frame.c_str(), source_frame.c_str(), e.what());
  		return false;
  	}
  	mrpt_bridge::convert(transform, des);
  	return true;
  }
  
  void PFLocalizationNode::callbackLaser(const sensor_msgs::LaserScan& _msg)
  {
  	using namespace mrpt::maps;
  	using namespace mrpt::obs;
  
  	time_last_input_ = ros::Time::now();
  
  	// ROS_INFO("callbackLaser");
  	auto laser = CObservation2DRangeScan::Create();
      
  	// printf("callbackLaser %s\n", _msg.header.frame_id.c_str());
  	if (laser_poses_.find(_msg.header.frame_id) == laser_poses_.end())
  	{
  		updateSensorPose(_msg.header.frame_id);
  	}
  	else if (state_ != IDLE)  // updating filter; we must be moving or
  	// update_while_stopped set to true
  	{
          if(param()->update_sensor_pose) {
              updateSensorPose(_msg.header.frame_id);
          }
  		// mrpt::poses::CPose3D pose = laser_poses_[_msg.header.frame_id];
  		// ROS_INFO("LASER POSE %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f",
  		// pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
  		mrpt_bridge::convert(_msg, laser_poses_[_msg.header.frame_id], *laser);
  
  		auto sf = CSensoryFrame::Create();
  		CObservationOdometry::Ptr odometry;
  		odometryForCallback(odometry, _msg.header);
  
  		CObservation::Ptr obs = CObservation::Ptr(laser);
  		sf->insert(obs);
  		observation(sf, odometry);
  		if (param()->gui_mrpt) show3DDebug(sf);
  	}
  }
  
  void PFLocalizationNode::callbackBeacon(
  	const mrpt_msgs::ObservationRangeBeacon& _msg)
  {
  	using namespace mrpt::maps;
  	using namespace mrpt::obs;
  
  	time_last_input_ = ros::Time::now();
  
  	// ROS_INFO("callbackBeacon");
  	auto beacon = CObservationBeaconRanges::Create();
  	// printf("callbackBeacon %s\n", _msg.header.frame_id.c_str());
  	if (beacon_poses_.find(_msg.header.frame_id) == beacon_poses_.end())
  	{
  		updateSensorPose(_msg.header.frame_id);
  	}
  	else if (state_ != IDLE)  // updating filter; we must be moving or
  	// update_while_stopped set to true
  	{
          if(param()->update_sensor_pose) {
              updateSensorPose(_msg.header.frame_id);
          }
  		// mrpt::poses::CPose3D pose = beacon_poses_[_msg.header.frame_id];
  		// ROS_INFO("BEACON POSE %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f",
  		// pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
  		mrpt_bridge::convert(
  			_msg, beacon_poses_[_msg.header.frame_id], *beacon);
  
  		auto sf = CSensoryFrame::Create();
  		CObservationOdometry::Ptr odometry;
  		odometryForCallback(odometry, _msg.header);
  
  		CObservation::Ptr obs = CObservation::Ptr(beacon);
  		sf->insert(obs);
  		observation(sf, odometry);
  		if (param()->gui_mrpt) show3DDebug(sf);
  	}
  }
  
  void PFLocalizationNode::callbackRobotPose(
  	const geometry_msgs::PoseWithCovarianceStamped& _msg)
  {
  	using namespace mrpt::maps;
  	using namespace mrpt::obs;
  
  	time_last_input_ = ros::Time::now();
  
  	// Robot pose externally provided; we update filter regardless state_
  	// attribute's value, as these
  	// corrections are typically independent from robot motion (e.g. inputs from
  	// GPS or tracking system)
  	// XXX admittedly an arbitrary choice; feel free to open an issue if you
  	// think it doesn't make sense
  
  	static std::string base_frame_id =
  		tf::resolve(param()->tf_prefix, param()->base_frame_id);
  	static std::string global_frame_id =
  		tf::resolve(param()->tf_prefix, param()->global_frame_id);
  
  	tf::StampedTransform map_to_obs_tf;
  	try
  	{
  		tf_listener_.waitForTransform(
  			global_frame_id, _msg.header.frame_id, ros::Time(0.0),
  			ros::Duration(0.5));
  		tf_listener_.lookupTransform(
  			global_frame_id, _msg.header.frame_id, ros::Time(0.0),
  			map_to_obs_tf);
  	}
  	catch (tf::TransformException& ex)
  	{
  		ROS_ERROR("%s", ex.what());
  		return;
  	}
  
  	// Transform observation into global frame, including covariance. For that,
  	// we must first obtain
  	// the global frame -> observation frame tf as a Pose msg, as required by
  	// pose_cov_ops::compose
  	geometry_msgs::Pose map_to_obs_pose;
  	tf::pointTFToMsg(map_to_obs_tf.getOrigin(), map_to_obs_pose.position);
  	tf::quaternionTFToMsg(
  		map_to_obs_tf.getRotation(), map_to_obs_pose.orientation);
  	geometry_msgs::PoseWithCovarianceStamped obs_pose_world;
  	obs_pose_world.header.stamp = _msg.header.stamp;
  	obs_pose_world.header.frame_id = global_frame_id;
  	pose_cov_ops::compose(map_to_obs_pose, _msg.pose, obs_pose_world.pose);
  
  	// Ensure the covariance matrix can be inverted (no zeros in the diagonal)
  	for (unsigned int i = 0; i < obs_pose_world.pose.covariance.size(); ++i)
  	{
  		if (i / 6 == i % 6 && obs_pose_world.pose.covariance[i] <= 0.0)
  			obs_pose_world.pose.covariance[i] =
  				std::numeric_limits<double>().infinity();
  	}
  
  	// Covert the received pose into an observation the filter can integrate
  	auto feature = CObservationRobotPose::Create();
  
  	feature->sensorLabel = _msg.header.frame_id;
  	mrpt_bridge::convert(_msg.header.stamp, feature->timestamp);
  	mrpt_bridge::convert(obs_pose_world.pose, feature->pose);
  
  	auto sf = CSensoryFrame::Create();
  	CObservationOdometry::Ptr odometry;
  	odometryForCallback(odometry, _msg.header);
  
  	CObservation::Ptr obs = CObservation::Ptr(feature);
  	sf->insert(obs);
  	observation(sf, odometry);
  	if (param()->gui_mrpt) show3DDebug(sf);
  }
  
  void PFLocalizationNode::odometryForCallback(
  	CObservationOdometry::Ptr& _odometry, const std_msgs::Header& _msg_header)
  {
  	std::string base_frame_id =
  		tf::resolve(param()->tf_prefix, param()->base_frame_id);
  	std::string odom_frame_id =
  		tf::resolve(param()->tf_prefix, param()->odom_frame_id);
  	mrpt::poses::CPose3D poseOdom;
  	if (this->waitForTransform(
  			poseOdom, odom_frame_id, base_frame_id, _msg_header.stamp,
  			ros::Duration(1.0)))
  	{
  		_odometry = CObservationOdometry::Create();
  		_odometry->sensorLabel = odom_frame_id;
  		_odometry->hasEncodersInfo = false;
  		_odometry->hasVelocities = false;
  		_odometry->odometry.x() = poseOdom.x();
  		_odometry->odometry.y() = poseOdom.y();
  		_odometry->odometry.phi() = poseOdom.yaw();
  	}
  }
  
  bool PFLocalizationNode::waitForMap()
  {
  	int wait_counter = 0;
  	int wait_limit = 10;
  
  	if (param()->use_map_topic)
  	{
  		sub_map_ =
  			nh_.subscribe("map", 1, &PFLocalizationNode::callbackMap, this);
  		ROS_INFO("Subscribed to map topic.");
  
  		while (!first_map_received_ && ros::ok() && wait_counter < wait_limit)
  		{
  			ROS_INFO("waiting for map callback..");
  			ros::Duration(0.5).sleep();
  			ros::spinOnce();
  			wait_counter++;
  		}
  		if (wait_counter != wait_limit)
  		{
  			return true;
  		}
  	}
  	else
  	{
  		client_map_ = nh_.serviceClient<nav_msgs::GetMap>("static_map");
  		nav_msgs::GetMap srv;
  		while (!client_map_.call(srv) && ros::ok() && wait_counter < wait_limit)
  		{
  			ROS_INFO("waiting for map service!");
  			ros::Duration(0.5).sleep();
  			wait_counter++;
  		}
  		client_map_.shutdown();
  		if (wait_counter != wait_limit)
  		{
  			ROS_INFO_STREAM("Map service complete.");
  			updateMap(srv.response.map);
  			return true;
  		}
  	}
  
  	ROS_WARN_STREAM("No map received.");
  	return false;
  }
  
  void PFLocalizationNode::callbackMap(const nav_msgs::OccupancyGrid& msg)
  {
  	if (param()->first_map_only && first_map_received_)
  	{
  		return;
  	}
  
  	ROS_INFO_STREAM("Map received.");
  	updateMap(msg);
  
  	first_map_received_ = true;
  }
  
  void PFLocalizationNode::updateSensorPose(std::string _frame_id)
  {
  	mrpt::poses::CPose3D pose;
  	tf::StampedTransform transform;
  	try
  	{
  		std::string base_frame_id =
  			tf::resolve(param()->tf_prefix, param()->base_frame_id);
  		tf_listener_.lookupTransform(
  			base_frame_id, _frame_id, ros::Time(0), transform);
  		tf::Vector3 translation = transform.getOrigin();
  		tf::Quaternion quat = transform.getRotation();
  		pose.x() = translation.x();
  		pose.y() = translation.y();
  		pose.z() = translation.z();
  		tf::Matrix3x3 Rsrc(quat);
  		mrpt::math::CMatrixDouble33 Rdes;
  		for (int c = 0; c < 3; c++)
  			for (int r = 0; r < 3; r++) Rdes(r, c) = Rsrc.getRow(r)[c];
  		pose.setRotationMatrix(Rdes);
  		laser_poses_[_frame_id] = pose;
  		beacon_poses_[_frame_id] = pose;
  	}
  	catch (tf::TransformException& ex)
  	{
  		ROS_ERROR("%s", ex.what());
  		ros::Duration(1.0).sleep();
  	}
  }
  
  void PFLocalizationNode::callbackInitialpose(
  	const geometry_msgs::PoseWithCovarianceStamped& _msg)
  {
  	const geometry_msgs::PoseWithCovariance& pose = _msg.pose;
  	mrpt_bridge::convert(pose, initial_pose_);
  	update_counter_ = 0;
  	state_ = INIT;
  }
  
  void PFLocalizationNode::callbackOdometry(const nav_msgs::Odometry& _msg)
  {
  	// We always update the filter if update_while_stopped is true, regardless
  	// robot is moving or
  	// not; otherwise, update filter if we are moving or at initialization (100
  	// first iterations)
  	bool moving = std::abs(_msg.twist.twist.linear.x) > 1e-3 ||
  				  std::abs(_msg.twist.twist.linear.y) > 1e-3 ||
  				  std::abs(_msg.twist.twist.linear.z) > 1e-3 ||
  				  std::abs(_msg.twist.twist.angular.x) > 1e-3 ||
  				  std::abs(_msg.twist.twist.angular.y) > 1e-3 ||
  				  std::abs(_msg.twist.twist.angular.z) > 1e-3;
  	if (param()->update_while_stopped || moving)
  	{
  		if (state_ == IDLE)
  		{
  			state_ = RUN;
  		}
  	}
  	else if (state_ == RUN && update_counter_ >= 100)
  	{
  		state_ = IDLE;
  	}
  }
  
  void PFLocalizationNode::updateMap(const nav_msgs::OccupancyGrid& _msg)
  {
  #if MRPT_VERSION >= 0x199
  	ASSERT_(metric_map_.countMapsByClass<COccupancyGridMap2D>());
  	mrpt_bridge::convert(_msg, *metric_map_.mapByClass<COccupancyGridMap2D>());
  #else
  	ASSERT_(metric_map_.m_gridMaps.size() == 1);
  	mrpt_bridge::convert(_msg, *metric_map_.m_gridMaps[0]);
  #endif
  }
  
  bool PFLocalizationNode::mapCallback(
  	nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
  {
  	ROS_INFO("mapCallback: service requested!\n");
  	res = resp_;
  	return true;
  }
  
  void PFLocalizationNode::publishMap()
  {
  	resp_.map.header.stamp = ros::Time::now();
  	resp_.map.header.frame_id =
  		tf::resolve(param()->tf_prefix, param()->global_frame_id);
  	resp_.map.header.seq = loop_count_;
  	if (pub_map_.getNumSubscribers() > 0)
  	{
  		pub_map_.publish(resp_.map);
  	}
  	if (pub_metadata_.getNumSubscribers() > 0)
  	{
  		pub_metadata_.publish(resp_.map.info);
  	}
  }
  
  void PFLocalizationNode::publishParticles()
  {
  	if (pub_particles_.getNumSubscribers() > 0)
  	{
  		geometry_msgs::PoseArray poseArray;
  		poseArray.header.frame_id =
  			tf::resolve(param()->tf_prefix, param()->global_frame_id);
  		poseArray.header.stamp = ros::Time::now();
  		poseArray.header.seq = loop_count_;
  		poseArray.poses.resize(pdf_.particlesCount());
  		for (size_t i = 0; i < pdf_.particlesCount(); i++)
  		{
  			mrpt::math::TPose2D p = pdf_.getParticlePose(i);
  			mrpt_bridge::convert(p, poseArray.poses[i]);
  		}
  		mrpt::poses::CPose2D p;
  		pub_particles_.publish(poseArray);
  	}
  }
  
  /**
   * @brief Publish map -> odom tf; as the filter provides map -> base, we
   * multiply it by base -> odom
   */
  void PFLocalizationNode::publishTF()
  {
  	static std::string base_frame_id =
  		tf::resolve(param()->tf_prefix, param()->base_frame_id);
  	static std::string odom_frame_id =
  		tf::resolve(param()->tf_prefix, param()->odom_frame_id);
  	static std::string global_frame_id =
  		tf::resolve(param()->tf_prefix, param()->global_frame_id);
  
  	mrpt::poses::CPose2D robot_pose;
  	pdf_.getMean(robot_pose);
  	tf::StampedTransform base_on_map_tf, odom_on_base_tf;
  	mrpt_bridge::convert(robot_pose, base_on_map_tf);
  	ros::Time time_last_update(0.0);
  	if (state_ == RUN)
  	{
  		mrpt_bridge::convert(time_last_update_, time_last_update);
  
  		// Last update time can be too far in the past if we where not updating
  		// filter, due to robot stopped or no
  		// observations for a while (we optionally show a warning in the second
  		// case)
  		// We use time zero if so when getting base -> odom tf to prevent an
  		// extrapolation into the past exception
  		if ((ros::Time::now() - time_last_update).toSec() >
  			param()->no_update_tolerance)
  		{
  			if ((ros::Time::now() - time_last_input_).toSec() >
  				param()->no_inputs_tolerance)
  			{
  				ROS_WARN_THROTTLE(
  					2.0,
  					"No observations received for %.2fs (tolerance %.2fs); are "
  					"robot sensors working?",
  					(ros::Time::now() - time_last_input_).toSec(),
  					param()->no_inputs_tolerance);
  			}
  			else
  			{
  				ROS_DEBUG_THROTTLE(
  					2.0,
  					"No filter updates for %.2fs (tolerance %.2fs); probably "
  					"robot stopped for a while",
  					(ros::Time::now() - time_last_update).toSec(),
  					param()->no_update_tolerance);
  			}
  
  			time_last_update = ros::Time(0.0);
  		}
  	}
  
  	try
  	{
  		// Get base -> odom transform
  		tf_listener_.waitForTransform(
  			base_frame_id, odom_frame_id, time_last_update, ros::Duration(0.1));
  		tf_listener_.lookupTransform(
  			base_frame_id, odom_frame_id, time_last_update, odom_on_base_tf);
  	}
  	catch (tf::TransformException& e)
  	{
  		ROS_WARN_THROTTLE(
  			2.0, "Transform from base frame (%s) to odom frame (%s) failed: %s",
  			base_frame_id.c_str(), odom_frame_id.c_str(), e.what());
  		ROS_WARN_THROTTLE(
  			2.0,
  			"Ensure that your mobile base driver is broadcasting %s -> %s tf",
  			odom_frame_id.c_str(), base_frame_id.c_str());
  		return;
  	}
  
  	// We want to send a transform that is good up until a tolerance time so
  	// that odom can be used
  	ros::Time transform_expiration =
  		(time_last_update.isZero() ? ros::Time::now() : time_last_update) +
  		ros::Duration(param()->transform_tolerance);
  	tf::StampedTransform tmp_tf_stamped(
  		base_on_map_tf * odom_on_base_tf, transform_expiration, global_frame_id,
  		odom_frame_id);
  	tf_broadcaster_.sendTransform(tmp_tf_stamped);
  }
  
  /**
   * @brief Publish the current pose of the robot
   **/
  void PFLocalizationNode::publishPose()
  {
  	// cov for x, y, phi (meter, meter, radian)
  #if MRPT_VERSION >= 0x199
  	const auto [cov, mean] = pdf_.getCovarianceAndMean();
  #else
  	mrpt::math::CMatrixDouble33 cov;
  	mrpt::poses::CPose2D mean;
  	pdf_.getCovarianceAndMean(cov, mean);
  #endif
  
  	geometry_msgs::PoseWithCovarianceStamped p;
  
  	// Fill in the header
  	p.header.frame_id =
  		tf::resolve(param()->tf_prefix, param()->global_frame_id);
  	if (loop_count_ < 10 || state_ == IDLE)
  		p.header.stamp = ros::Time::now();  // on first iterations timestamp
  	// differs a lot from ROS time
  	else
  		mrpt_bridge::convert(time_last_update_, p.header.stamp);
  
  	// Copy in the pose
  	mrpt_bridge::convert(mean, p.pose.pose);
  
  	// Copy in the covariance, converting from 3-D to 6-D
  	for (int i = 0; i < 3; i++)
  	{
  		for (int j = 0; j < 3; j++)
  		{
  			int ros_i = i;
  			int ros_j = j;
  			if (i == 2 || j == 2)
  			{
  				ros_i = i == 2 ? 5 : i;
  				ros_j = j == 2 ? 5 : j;
  			}
  			p.pose.covariance[ros_i * 6 + ros_j] = cov(i, j);
  		}
  	}
  
  	pub_pose_.publish(p);
  }
  
  void PFLocalizationNode::useROSLogLevel()
  {
  	// Set ROS log level also on MRPT internal log system; level enums are fully
  	// compatible
  	std::map<std::string, ros::console::levels::Level> loggers;
  	ros::console::get_loggers(loggers);
  	if (loggers.find("ros.roscpp") != loggers.end())
  		pdf_.setVerbosityLevel(
  			static_cast<VerbosityLevel>(loggers["ros.roscpp"]));
  	if (loggers.find("ros.mrpt_localization") != loggers.end())
  		pdf_.setVerbosityLevel(
  			static_cast<VerbosityLevel>(loggers["ros.mrpt_localization"]));
  }
  