CGraphSlamHandler_ROS_impl.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+
 */
#pragma once

#include <mrpt/ros1bridge/time.h>
#include <mrpt/ros1bridge/laser_scan.h>
#include <mrpt/ros1bridge/pose.h>
#include <mrpt/ros1bridge/map.h>

namespace mrpt
{
namespace graphslam
{
namespace apps
{
// From:
// https://answers.ros.org/question/364561/tfcreatequaternionfromyaw-equivalent-in-ros2/
static inline auto createQuaternionMsgFromYaw(double yaw)
{
        tf2::Quaternion q;
        q.setRPY(0, 0, yaw);
        return tf2::toMsg(q);
}

// static member variables
template <class GRAPH_T>
const std::string CGraphSlamHandler_ROS<GRAPH_T>::sep_header(40, '=');

template <class GRAPH_T>
const std::string CGraphSlamHandler_ROS<GRAPH_T>::sep_subheader(20, '-');

// Ctor
template <class GRAPH_T>
CGraphSlamHandler_ROS<GRAPH_T>::CGraphSlamHandler_ROS(
        mrpt::system::COutputLogger* logger,
        TUserOptionsChecker<GRAPH_T>* options_checker, ros::NodeHandle* nh_in)
        : parent_t(logger, options_checker, /*enable_visuals=*/false), m_nh(nh_in)
{
        using namespace mrpt::obs;

        ASSERT_(m_nh);

        // TODO - does this affect?
        // Previous value = 0;
        m_queue_size = 1;

        // variables initialization/assignment
        m_pub_seq = 0;
        m_stats_pub_seq = 0;
        this->resetReceivedFlags();

        // measurements initialization
        m_mrpt_odom = CObservationOdometry::Create();
        m_mrpt_odom->hasEncodersInfo = false;
        m_mrpt_odom->hasVelocities = false;
        m_first_time_in_sniff_odom = true;

        m_measurement_cnt = 0;

        // Thu Nov 3 23:36:49 EET 2016, Nikos Koukis
        // WARNING: ROS Server Parameters have not been read yet. Make sure you know
        // what to initialize at this stage!
}

template <class GRAPH_T>
CGraphSlamHandler_ROS<GRAPH_T>::~CGraphSlamHandler_ROS()
{
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::readParams()
{
        this->readROSParameters();

        ASSERT_(!this->m_ini_fname.empty());
        parent_t::readConfigFname(this->m_ini_fname);
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::readROSParameters()
{
        // misc
        {
                std::string ns = "misc/";

                // enable/disable visuals
                bool m_disable_MRPT_visuals;
                m_nh->param<bool>(
                        ns + "disable_MRPT_visuals", m_disable_MRPT_visuals, false);
                this->m_enable_visuals = !m_disable_MRPT_visuals;

                // verbosity level
                int lvl;
                m_nh->param<int>(ns + "verbosity", lvl, static_cast<int>(LVL_INFO));
                m_min_logging_level = static_cast<VerbosityLevel>(lvl);
                this->m_logger->setMinLoggingLevel(m_min_logging_level);
        }
        // deciders, optimizer
        {
                std::string ns = "deciders_optimizers/";
                m_nh->param<std::string>(ns + "NRD", m_node_reg, "CFixedIntervalsNRD");
                m_nh->param<std::string>(ns + "ERD", m_edge_reg, "CICPCriteriaERD");
                m_nh->param<std::string>(ns + "GSO", m_optimizer, "CLevMarqGSO");
        }
        // filenames
        {
                std::string ns = "files/";

                // configuration file - mandatory
                std::string config_param_path = ns + "config";
                bool found_config = m_nh->getParam(ns + "config", this->m_ini_fname);
                ASSERTMSG_(
                        found_config, mrpt::format(
                                                          "Configuration file was not set. Set %s and try "
                                                          "again.\nExiting...",
                                                          config_param_path.c_str()));

                // ground-truth file
                m_nh->getParam(ns + "ground_truth", this->m_gt_fname);
        }

        // TF Frame IDs
        // names of the frames of the corresponding robot parts
        {
                std::string ns = "frame_IDs/";

                m_nh->param<std::string>(ns + "anchor_frame", m_anchor_frame_id, "map");
                m_nh->param<std::string>(
                        ns + "base_link_frame", m_base_link_frame_id, "base_link");
                m_nh->param<std::string>(
                        ns + "odometry_frame", m_odom_frame_id, "odom");
        }

        // ASSERT that the given user options are valid
        // Fill the TuserOptionsChecker related structures
        this->m_options_checker->createDeciderOptimizerMappings();
        this->m_options_checker->populateDeciderOptimizerProperties();
        this->verifyUserInput();

        this->m_logger->logFmt(
                LVL_DEBUG, "Successfully read parameters from ROS Parameter Server");

        // Visuals initialization
        if (this->m_enable_visuals)
        {
                this->initVisualization();
        }
}  // end of readROSParameters

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::readStaticTFs()
{
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::initEngine_ROS()
{
        this->m_logger->logFmt(
                LVL_WARN, "Initializing CGraphSlamEngine_ROS instance...");
        this->m_engine = new CGraphSlamEngine_ROS<GRAPH_T>(
                m_nh, this->m_ini_fname,
                /*rawlog_fname=*/"", this->m_gt_fname, this->m_win_manager,
                this->m_options_checker->node_regs_map[m_node_reg](),
                this->m_options_checker->edge_regs_map[m_edge_reg](),
                this->m_options_checker->optimizers_map[m_optimizer]());
        this->m_logger->logFmt(
                LVL_WARN, "Successfully initialized CGraphSlamEngine_ROS instance.");
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::initEngine_MR()
{
        this->m_options_checker->node_regs_map[m_node_reg]();
        this->m_options_checker->edge_regs_map[m_edge_reg]();
        this->m_options_checker->optimizers_map[m_optimizer]();

        this->m_logger->logFmt(
                LVL_WARN, "Initializing CGraphSlamEngine_MR instance...");
        this->m_engine = new CGraphSlamEngine_MR<GRAPH_T>(
                m_nh, this->m_ini_fname,
                /*rawlog_fname=*/"", this->m_gt_fname, this->m_win_manager,
                this->m_options_checker->node_regs_map[m_node_reg](),
                this->m_options_checker->edge_regs_map[m_edge_reg](),
                this->m_options_checker->optimizers_map[m_optimizer]());
        this->m_logger->logFmt(
                LVL_WARN, "Successfully initialized CGraphSlamEngine_MR instance.");
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::getROSParameters(std::string* str_out)
{
        using namespace std;

        ASSERT_(str_out);

        stringstream ss("");

        ss << "ROS Parameters: " << endl;
        ss << sep_header << endl;
        ss << endl;

        ss << "Deciders / Optimizers = " << endl;
        ss << sep_subheader << endl;
        ss << "Node Registration Decider = " << m_node_reg << endl;
        ss << "Edge Registration Decider = " << m_edge_reg << endl;
        ss << "GraphSLAM Optimizer       = " << m_optimizer << endl;
        ss << endl;

        ss << "Filenames: " << endl;
        ss << sep_subheader << endl;
        ss << "Configuration .ini file   = " << this->m_ini_fname << endl;
        ss << "Ground truth filename     = "
           << (!this->m_gt_fname.empty() ? this->m_gt_fname : "NONE") << endl;
        ss << endl;

        ss << "Miscellaneous: " << endl;
        ss << sep_subheader << endl;
        ss << "Enable MRPT visuals?      = "
           << (this->m_enable_visuals ? "TRUE" : "FALSE") << endl;
        ss << "Logging verbosity Level   = "
           << COutputLogger::logging_levels_to_names()[m_min_logging_level] << endl;

        ss << endl;

        *str_out = ss.str();
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::getParamsAsString(std::string* str_out)
{
        ASSERT_(str_out);

        // ros parameters
        std::string ros_params("");
        this->getROSParameters(&ros_params);
        std::string mrpt_params("");
        parent_t::getParamsAsString(&mrpt_params);

        *str_out += ros_params;
        *str_out += "\n\n";
        *str_out += mrpt_params;

        // various parameters
}

template <class GRAPH_T>
std::string CGraphSlamHandler_ROS<GRAPH_T>::getParamsAsString()
{
        std::string params;
        this->getParamsAsString(&params);
        return params;
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::printParams()
{
        using namespace std;
        parent_t::printParams();
        cout << this->getParamsAsString() << endl;
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::verifyUserInput()
{
        this->m_logger->logFmt(LVL_DEBUG, "Verifying user input...");

        // verify the NRD, ERD, GSO parameters
        bool node_success, edge_success, optimizer_success;
        bool failed = false;

        node_success = this->m_options_checker->checkRegistrationDeciderExists(
                m_node_reg, "node");
        edge_success = this->m_options_checker->checkRegistrationDeciderExists(
                m_edge_reg, "edge");
        optimizer_success =
                this->m_options_checker->checkOptimizerExists(m_optimizer);

        if (!node_success)
        {
                this->m_logger->logFmt(
                        LVL_ERROR, "\nNode Registration Decider \"%s\" is not available",
                        m_node_reg.c_str());
                this->m_options_checker->dumpRegistrarsToConsole("node");
                failed = true;
        }
        if (!edge_success)
        {
                this->m_logger->logFmt(
                        LVL_ERROR, "\nEdge Registration Decider \"%s\" is not available.",
                        m_edge_reg.c_str());
                this->m_options_checker->dumpRegistrarsToConsole("edge");
                failed = true;
        }
        if (!optimizer_success)
        {
                this->m_logger->logFmt(
                        LVL_ERROR, "\ngraphSLAM Optimizser \"%s\" is not available.",
                        m_optimizer.c_str());
                this->m_options_checker->dumpOptimizersToConsole();
                failed = true;
        }
        ASSERT_(!failed);

        // verify that the path to the files is correct
        // .ini file
        bool ini_exists = system::fileExists(this->m_ini_fname);
        ASSERTMSG_(
                ini_exists, format(
                                                "\n.ini configuration file \"%s\"doesn't exist. "
                                                "Please specify a valid pathname.\nExiting...\n",
                                                this->m_ini_fname.c_str()));
        // ground-truth file
        if (!this->m_gt_fname.empty())
        {
                bool gt_exists = system::fileExists(this->m_gt_fname);
                ASSERTMSG_(
                        gt_exists, format(
                                                   "\nGround truth file \"%s\"doesn't exist."
                                                   "Either unset the corresponding ROS parameter or "
                                                   "specify a valid pathname.\n"
                                                   "Exiting...\n",
                                                   this->m_gt_fname.c_str()));
        }

}  // end of verifyUserInput

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::setupComm()
{
        this->m_logger->logFmt(
                LVL_INFO,
                "Setting up ROS-related subscribers, publishers, services...");

        // setup subscribers, publishers, services...
        this->setupSubs();
        this->setupPubs();
        this->setupSrvs();

        // fetch the static geometrical transformations
        // this->readStaticTFs();

}  // end of setupComm

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::setupSubs()
{
        this->m_logger->logFmt(LVL_INFO, "Setting up the subscribers...");

        // setup the names
        std::string ns = "input/";

        m_odom_topic = ns + "odom";
        m_laser_scan_topic = ns + "laser_scan";

        // odometry
        m_odom_sub = m_nh->subscribe<nav_msgs::Odometry>(
                m_odom_topic, m_queue_size, &self_t::sniffOdom, this);

        // laser_scans
        m_laser_scan_sub = m_nh->subscribe<sensor_msgs::LaserScan>(
                m_laser_scan_topic, m_queue_size, &self_t::sniffLaserScan, this);

        // camera
        // TODO

        // 3D point clouds
        // TODO

}  // end of setupSubs

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::setupPubs()
{
        this->m_logger->logFmt(LVL_INFO, "Setting up the publishers...");

        // setup the names
        std::string ns = "feedback/";

        m_curr_robot_pos_topic = ns + "robot_position";
        m_robot_trajectory_topic = ns + "robot_trajectory";
        m_robot_tr_poses_topic = ns + "robot_tr_poses";
        m_odom_trajectory_topic = ns + "odom_trajectory";
        m_gridmap_topic = ns + "gridmap";
        m_stats_topic = ns + "graphslam_stats";

        // setup the publishers

        // agent estimated position
        m_curr_robot_pos_pub = m_nh->advertise<geometry_msgs::PoseStamped>(
                m_curr_robot_pos_topic, m_queue_size, true);
        m_robot_trajectory_pub = m_nh->advertise<nav_msgs::Path>(
                m_robot_trajectory_topic, m_queue_size, true);
        m_robot_tr_poses_pub = m_nh->advertise<geometry_msgs::PoseArray>(
                m_robot_tr_poses_topic, m_queue_size, true);

        // odometry nav_msgs::Path
        m_odom_path.header.seq = 0;
        m_odom_path.header.stamp = ros::Time::now();
        m_odom_path.header.frame_id = m_anchor_frame_id;

        m_odom_trajectory_pub =
                m_nh->advertise<nav_msgs::Path>(m_odom_trajectory_topic, m_queue_size);

        // generated gridmap
        m_gridmap_pub = m_nh->advertise<nav_msgs::OccupancyGrid>(
                m_gridmap_topic, m_queue_size,
                /*latch=*/true);

        m_stats_pub = m_nh->advertise<mrpt_msgs::GraphSlamStats>(
                m_stats_topic, m_queue_size,
                /*latch=*/true);

}  // end of setupPubs

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::setupSrvs()
{
        this->m_logger->logFmt(LVL_INFO, "Setting up the services...");

        // TODO Error statistics

}  // end of setupSrvs

template <class GRAPH_T>
bool CGraphSlamHandler_ROS<GRAPH_T>::usePublishersBroadcasters()
{
        using namespace std;

        MRPT_START;
        bool ret_val = true;

        ros::Time timestamp = ros::Time::now();

        // current MRPT robot pose
        pose_t mrpt_pose = this->m_engine->getCurrentRobotPosEstimation();

        //
        // convert pose_t to corresponding geometry_msg::TransformStamped
        // anchor frame <=> base_link
        //
        geometry_msgs::TransformStamped anchor_base_link_transform;
        anchor_base_link_transform.header.stamp = ros::Time::now();
        anchor_base_link_transform.header.frame_id = m_anchor_frame_id;
        anchor_base_link_transform.child_frame_id = m_base_link_frame_id;

        // translation
        anchor_base_link_transform.transform.translation.x = mrpt_pose.x();
        anchor_base_link_transform.transform.translation.y = mrpt_pose.y();
        anchor_base_link_transform.transform.translation.z = 0;

        // rotation
        anchor_base_link_transform.transform.rotation =
                createQuaternionMsgFromYaw(mrpt_pose.phi());

        // TODO - potential error in the rotation, investigate this
        m_broadcaster.sendTransform(anchor_base_link_transform);

        // anchor frame <=> odom frame
        //
        // make sure that we have received odometry information in the first
        // place...
        // the corresponding field would be initialized
        if (!m_anchor_odom_transform.child_frame_id.empty())
        {
                m_broadcaster.sendTransform(m_anchor_odom_transform);
        }

        // set an arrow indicating the current orientation of the robot
        {
                geometry_msgs::PoseStamped geom_pose;
                geom_pose.header.stamp = timestamp;
                geom_pose.header.seq = m_pub_seq;
                geom_pose.header.frame_id =
                        m_anchor_frame_id;      // with regards to base_link...

                // position
                geom_pose.pose = mrpt::ros1bridge::toROS_Pose(mrpt_pose);
                m_curr_robot_pos_pub.publish(geom_pose);
        }

        // robot trajectory
        // publish the trajectory of the robot
        {
                this->m_logger->logFmt(
                        LVL_DEBUG, "Publishing the current robot trajectory");
                typename GRAPH_T::global_poses_t graph_poses;
                graph_poses = this->m_engine->getRobotEstimatedTrajectory();

                nav_msgs::Path path;

                // set the header
                path.header.stamp = timestamp;
                path.header.seq = m_pub_seq;
                path.header.frame_id = m_anchor_frame_id;

                //
                // fill in the pose as well as the nav_msgs::Path at the same time.
                //

                geometry_msgs::PoseArray geom_poses;
                geom_poses.header.stamp = timestamp;
                geom_poses.header.frame_id = m_anchor_frame_id;

                for (auto n_cit = graph_poses.begin(); n_cit != graph_poses.end();
                         ++n_cit)
                {
                        geometry_msgs::PoseStamped geom_pose_stamped;
                        geometry_msgs::Pose geom_pose;

                        // grab the pose - convert to geometry_msgs::Pose format
                        const pose_t& mrpt_pose = n_cit->second;
                        geom_pose = mrpt::ros1bridge::toROS_Pose(mrpt_pose);
                        geom_poses.poses.push_back(geom_pose);
                        geom_pose_stamped.pose = geom_pose;

                        // edit the header
                        geom_pose_stamped.header.stamp = timestamp;
                        geom_pose_stamped.header.seq = m_pub_seq;
                        geom_pose_stamped.header.frame_id = m_anchor_frame_id;

                        path.poses.push_back(geom_pose_stamped);
                }

                // publish only on new node addition
                if (this->m_engine->getGraph().nodeCount() > m_graph_nodes_last_size)
                {
                        m_robot_tr_poses_pub.publish(geom_poses);
                        m_robot_trajectory_pub.publish(path);
                }
        }

        // Odometry trajectory - nav_msgs::Path
        m_odom_trajectory_pub.publish(m_odom_path);

        // generated gridmap
        // publish only on new node addition
        if (this->m_engine->getGraph().nodeCount() > m_graph_nodes_last_size)
        {
                std_msgs::Header h;
                mrpt::system::TTimeStamp mrpt_time;
                auto mrpt_gridmap = mrpt::maps::COccupancyGridMap2D::Create();
                this->m_engine->getMap(mrpt_gridmap, &mrpt_time);

                // timestamp
                h.stamp = mrpt::ros1bridge::toROS(mrpt_time);
                h.seq = m_pub_seq;
                h.frame_id = m_anchor_frame_id;

                // nav gridmap
                nav_msgs::OccupancyGrid nav_gridmap;
                mrpt::ros1bridge::toROS(*mrpt_gridmap, nav_gridmap, h);
                m_gridmap_pub.publish(nav_gridmap);
        }

        // GraphSlamStats publishing
        {
                mrpt_msgs::GraphSlamStats stats;
                stats.header.seq = m_stats_pub_seq++;

                map<string, int> node_stats;
                map<string, int> edge_stats;
                vector<double> def_energy_vec;
                mrpt::system::TTimeStamp mrpt_time;

                this->m_engine->getGraphSlamStats(&node_stats, &edge_stats, &mrpt_time);

                // node/edge count
                stats.nodes_total = node_stats["nodes_total"];
                stats.edges_total = edge_stats["edges_total"];
                if (edge_stats.find("ICP2D") != edge_stats.end())
                {
                        stats.edges_icp_2d = edge_stats["ICP2D"];
                }
                if (edge_stats.find("ICP3D") != edge_stats.end())
                {
                        stats.edges_icp_3d = edge_stats["ICP3D"];
                }
                if (edge_stats.find("Odometry") != edge_stats.end())
                {
                        stats.edges_odom = edge_stats["Odometry"];
                }
                stats.loop_closures = edge_stats["loop_closures"];

                // SLAM evaluation metric
                this->m_engine->getDeformationEnergyVector(
                        &stats.slam_evaluation_metric);

                stats.header.stamp = mrpt::ros1bridge::toROS(mrpt_time);

                m_stats_pub.publish(stats);
        }

        // update the last known size
        m_graph_nodes_last_size = this->m_engine->getGraph().nodeCount();
        m_pub_seq++;

        if (this->m_enable_visuals)
        {
                ret_val = this->queryObserverForEvents();
        }

        return ret_val;
        MRPT_END;
}  // end of usePublishersBroadcasters

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::sniffLaserScan(
        const sensor_msgs::LaserScan::ConstPtr& ros_laser_scan)
{
        using namespace std;
        using namespace mrpt::obs;

        this->m_logger->logFmt(
                LVL_DEBUG,
                "sniffLaserScan: Received a LaserScan msg. Converting it to MRPT "
                "format...");

        // build the CObservation2DRangeScan
        m_mrpt_laser_scan = CObservation2DRangeScan::Create();
        mrpt::poses::CPose3D rel_pose;  // pose is 0.
        mrpt::ros1bridge::fromROS(*ros_laser_scan, rel_pose, *m_mrpt_laser_scan);

        m_received_laser_scan = true;
        CObservation::Ptr tmp =
                mrpt::ptr_cast<CObservation>::from(m_mrpt_laser_scan);
        this->processObservation(tmp);
}  // end of sniffLaserScan

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::sniffOdom(
        const nav_msgs::Odometry::ConstPtr& ros_odom)
{
        using namespace std;
        using namespace mrpt::obs;
        using namespace mrpt::poses;

        this->m_logger->logFmt(
                LVL_DEBUG,
                "sniffOdom: Received an odometry msg. Converting it to MRPT format...");

        // update the odometry frame with regards to the anchor
        {
                // header
                m_anchor_odom_transform.header.frame_id = m_anchor_frame_id;
                m_anchor_odom_transform.header.stamp = ros_odom->header.stamp;
                m_anchor_odom_transform.header.seq = ros_odom->header.seq;

                m_anchor_odom_transform.child_frame_id = m_odom_frame_id;

                //
                // copy ros_odom ==> m_anchor_odom
                //

                // translation
                m_anchor_odom_transform.transform.translation.x =
                        ros_odom->pose.pose.position.x;
                m_anchor_odom_transform.transform.translation.y =
                        ros_odom->pose.pose.position.y;
                m_anchor_odom_transform.transform.translation.z =
                        ros_odom->pose.pose.position.z;

                // quaternion
                m_anchor_odom_transform.transform.rotation =
                        ros_odom->pose.pose.orientation;
        }

        // build and fill an MRPT CObservationOdometry instance for manipulation
        // from the main algorithm
        m_mrpt_odom->timestamp = mrpt::ros1bridge::fromROS(ros_odom->header.stamp);
        m_mrpt_odom->odometry =
                mrpt::poses::CPose2D(mrpt::ros1bridge::fromROS(ros_odom->pose.pose));

        // if this is the first call odometry should be 0. Decrement by the
        // corresponding offset
        if (m_first_time_in_sniff_odom)
        {
                m_input_odometry_offset = m_mrpt_odom->odometry;
                m_first_time_in_sniff_odom = false;
        }
        // decrement by the (constant) offset
        m_mrpt_odom->odometry = m_mrpt_odom->odometry - m_input_odometry_offset;

        // add to the overall odometry path
        {
                geometry_msgs::PoseStamped pose_stamped;
                pose_stamped.header = ros_odom->header;

                // just for convenience - convert the MRPT pose back to PoseStamped
                pose_stamped.pose = mrpt::ros1bridge::toROS_Pose(m_mrpt_odom->odometry);
                m_odom_path.poses.push_back(pose_stamped);
        }

        // print the odometry -  for debugging reasons...
        stringstream ss;
        ss << "Odometry - MRPT format:\t" << m_mrpt_odom->odometry << endl;
        this->m_logger->logFmt(LVL_DEBUG, "%s", ss.str().c_str());

        m_received_odom = true;
        CObservation::Ptr tmp =
                mrpt::ptr_cast<mrpt::obs::CObservation>::from(m_mrpt_odom);
        this->processObservation(tmp);
}  // end of sniffOdom

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::sniffCameraImage()
{
        THROW_EXCEPTION("Method is not implemented yet.");
}
template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::sniff3DPointCloud()
{
        THROW_EXCEPTION("Method is not implemented yet.");
}
template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::processObservation(
        mrpt::obs::CObservation::Ptr& observ)
{
        using namespace std;

        this->_process(observ);
        this->resetReceivedFlags();
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::_process(
        mrpt::obs::CObservation::Ptr& observ)
{
        // this->m_logger->logFmt(LVL_DEBUG, "Calling execGraphSlamStep...");

        // TODO - use the exit code of execGraphSlamStep to exit??
        if (!this->m_engine->isPaused())
        {
                this->m_engine->execGraphSlamStep(observ, m_measurement_cnt);
                m_measurement_cnt++;
        }
}

template <class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::resetReceivedFlags()
{
        m_received_odom = false;
        m_received_laser_scan = false;
        m_received_camera = false;
        m_received_point_cloud = false;
}

}  // namespace apps
}  // namespace graphslam
}  // namespace mrpt