diff --git a/CMakeLists.txt b/CMakeLists.txt
index d86286f103b61f77bfed474e450c52e037f57a04..658d0c6761774e04c906ea4a4d9a9f4748ce01b6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_adc_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm visualization_msgs geometry_msgs tf2_ros iri_adc_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -62,7 +62,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm iri_adc_msgs
+ CATKIN_DEPENDS iri_base_algorithm visualization_msgs geometry_msgs tf2_ros iri_adc_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -99,6 +99,8 @@ target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} visualization_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} iri_adc_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
diff --git a/README.md b/README.md
index b6eefcdf431379e57ee488da68debb9441431ca4..dad9720121cf84cc615db90a592802f1f4b1820b 100644
--- a/README.md
+++ b/README.md
@@ -3,7 +3,14 @@
 The iri_adc_landmarks_slam_solver project description
 
 # ROS Interface
+### Topic publishers
+  - ~**frame_data** (visualization_msgs/MarkerArray.msg)
+  - ~**landmarks** (visualization_msgs/MarkerArray.msg)
+  - ~**landmarks_localization_pose** (geometry_msgs/PoseWithCovarianceStamped.msg)
+  - /**tf** (tf/tfMessage)
 ### Topic subscribers
+  - ~**estimated_pose** (geometry_msgs/PoseWithCovarianceStamped.msg)
+  - /**tf** (tf/tfMessage)
   - ~**landmarks** (iri_adc_msgs/landmark_array.msg)
 
 ### Parameters
diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index adae733de88215dc4643fa6cb3439acf2ace953d..0ac19ce2406345e7ee00848a83175490266f3950 100755
--- a/cfg/AdcLandmarksSlamSolver.cfg
+++ b/cfg/AdcLandmarksSlamSolver.cfg
@@ -76,8 +76,8 @@ flags.add("amcl_localization",                    bool_t,       0,           "Bo
 flags.add("calculate_covariance",                 bool_t,       0,           "Boolean to calculate robot pose covariance",                                False)
 flags.add("publish_tf_map_odom",                  bool_t,       0,           "Boolean to publish tf from map to odom",                                    False)
 
-sensor_noise.add("laser_sigma_th",                double_t,     0,            "Sensor angular sigma",                                                       0.035, 0.001, 0.1)
-sensor_noise.add("laser_sigma_r",                 double_t,     0,            "Sensor radial sigma",                                                        0.05, 0.01, 0.2)
+sensor_noise.add("sensor_sigma_th",                double_t,     0,            "Sensor angular sigma",                                                       0.035, 0.001, 0.1)
+sensor_noise.add("sensor_sigma_r",                 double_t,     0,            "Sensor radial sigma",                                                        0.05, 0.01, 0.2)
 
 odom_noise.add("odom_fxy",                      double_t,     0,            "Odom linear sigma factor",                                                   0.05, 0.01, 1.0)
 odom_noise.add("odom_fth",                      double_t,     0,            "Odom angular sigma factor",                                                  0.05, 0.01, 1.0)
diff --git a/include/adc_landmarks_slam_solver_alg_node.h b/include/adc_landmarks_slam_solver_alg_node.h
index 36716708c596e1d355bad772abc01b7b4279da8a..259c0fc8c9d881e60c5cb261f10c9ed000ffd836 100644
--- a/include/adc_landmarks_slam_solver_alg_node.h
+++ b/include/adc_landmarks_slam_solver_alg_node.h
@@ -27,14 +27,35 @@
 
 #include <iri_base_algorithm/iri_base_algorithm.h>
 #include "adc_landmarks_slam_solver_alg.h"
+#include <sstream>
+#include <iostream>
+#include <fstream>
 
 // [publisher subscriber headers]
+#include <visualization_msgs/MarkerArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
 #include <iri_adc_msgs/feature_array.h>
 
 // [service client headers]
 
 // [action server client headers]
 
+/**
+ * \struct Landmark_info.
+ *
+ * \brief A struct to save the useful landmark info.
+ *
+ */
+typedef struct {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  Eigen::Vector3d pose; ///< Global position x, y and yaw.
+  bool detected; ///< If it has been detected.
+  int type; ///< Feature type.
+}Landmark_info;
+
 /**
  * \struct Feature_info.
  *
@@ -58,13 +79,54 @@ typedef struct {
 class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<AdcLandmarksSlamSolverAlgorithm>
 {
   private:
+    bool init_tf_map_odom; ///< Boolean to know if the initial tf map odom was received.
+    Eigen::Vector3d tf_map_odom; ///< Odom's x, y and rotation from fixed frame.
+
     bool new_features_data; ///< Bool to know that new features has been received.
     std::string features_frame; ///< Features origin frame.
     ros::Time features_stamp; ///< Time stamp from the features.
     std::vector<Feature_info> features; ///< Feature data vector.
+
+    std::map<double, Landmark_info> landmarks; ///< Landmarks.
+
+    ros::Time last_update_t; ///< Last frame update time.
+    Eigen::Vector3d last_update_odom; ///< Last frame update odometry.
+    bool update_problem; ///< Flag to update and solve the Ceres problem.
+
+    ros::Time last_publish_t; ///< Last publish pose time.
+    Eigen::Vector3d last_publish_odom; ///< Last publish pose odometry.
+    bool publish_pose; ///< Flag to publish pose.
+
+    bool new_estimated_pose; ///< Boolean to know there is a new estimated pose.
+    ros::Time new_estimated_pose_t; ///< New estimated pose time stamp.
+    Eigen::Vector3d estimated_pose; ///< Estimated pose;
+    Eigen::Vector3d estimated_sigma; ///< Estimated main sigmas;
+
     // [publisher attributes]
+    ros::Publisher frame_data_publisher_;
+    visualization_msgs::MarkerArray frame_data_MarkerArray_msg_;
+
+    ros::Publisher landmarks_publisher_;
+    visualization_msgs::MarkerArray landmarks_MarkerArray_msg_;
+
+    ros::Publisher landmarks_localization_pose_publisher_;
+    geometry_msgs::PoseWithCovarianceStamped landmarks_localization_pose_PoseWithCovarianceStamped_msg_;
+
+    tf2_ros::TransformBroadcaster tf2_broadcaster;
+    geometry_msgs::TransformStamped tf_map_odom_msg;
+
 
     // [subscriber attributes]
+    ros::Subscriber estimated_pose_subscriber_;
+    void estimated_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
+    pthread_mutex_t estimated_pose_mutex_;
+    void estimated_pose_mutex_enter(void);
+    void estimated_pose_mutex_exit(void);
+
+    tf2_ros::Buffer tf2_buffer;
+
+    tf2_ros::TransformListener tf2_listener;
+
     ros::Subscriber features_subscriber_;
     void features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg);
     pthread_mutex_t features_mutex_;
@@ -80,7 +142,10 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
 
     // [action client attributes]
 
-    void get_feature_msg_info();
+    /**
+     * \brief Function to load landmarks map from a txt file.
+     */
+    void load_landmarks(void);
 
    /**
     * \brief config variable
diff --git a/package.xml b/package.xml
index 90ffc3b7205f54bb45c1d22bc47ef4cc3847179d..0dd83d2e2d59c7cf31a441e5b4ef594f69d2d1c0 100644
--- a/package.xml
+++ b/package.xml
@@ -52,6 +52,9 @@
   <build_depend>iri_base_algorithm</build_depend>
   <build_export_depend>iri_base_algorithm</build_export_depend>
   <exec_depend>iri_base_algorithm</exec_depend>
+  <depend>visualization_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>iri_adc_msgs</depend>
 
 
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index 7ae16b52a6c02c7fc01e2c35f642fabfd7ee6c84..af464bc1caaeb4bf3c408098fec83e4173cd1a95 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -2,8 +2,11 @@
 
 AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<AdcLandmarksSlamSolverAlgorithm>()
+  ,tf2_listener(tf2_buffer)
 {
   //init class attributes if necessary
+
+  //Get general parameters.
   if(!this->private_node_handle_.getParam("rate", this->config_.rate))
   {
     ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'rate' not found");
@@ -11,13 +14,115 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
   else
     this->setRate(this->config_.rate);
 
+  if(!this->private_node_handle_.getParam("global_frame", this->config_.global_frame))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'global_frame' not found. Setting it to map.");
+    this->config_.global_frame = "map";
+  }
+
+  if(!this->private_node_handle_.getParam("odom_frame", this->config_.odom_frame))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'odom_frame' not found. Setting it to odom.");
+    this->config_.odom_frame = "odom";
+  }
+
+  if(!this->private_node_handle_.getParam("base_link_frame", this->config_.base_link_frame))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'base_link_frame' not found. Setting it to base_link.");
+    this->config_.base_link_frame = "base_link";
+  }
+
+  //Get flags
+  if(!this->private_node_handle_.getParam("load_landmarks", this->config_.load_landmarks))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'load_landmarks' not found. Setting it to false.");
+    this->config_.load_landmarks = false;
+  }
+
+  if(!this->private_node_handle_.getParam("landmarks_pos_fixed", this->config_.landmarks_pos_fixed))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'landmarks_pos_fixed' not found. Setting it to false.");
+    this->config_.landmarks_pos_fixed = false;
+  }
+
+  if(!this->private_node_handle_.getParam("search_for_new_landmarks", this->config_.search_for_new_landmarks))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'search_for_new_landmarks' not found. Setting it to false.");
+    this->config_.search_for_new_landmarks = false;
+  }
+
+  if(!this->private_node_handle_.getParam("first_robot_state_fixed", this->config_.first_robot_state_fixed))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'first_robot_state_fixed' not found. Setting it to false.");
+    this->config_.first_robot_state_fixed = false;
+  }
+
+  if(!this->private_node_handle_.getParam("last_robot_state_fixed", this->config_.last_robot_state_fixed))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'last_robot_state_fixed' not found. Setting it to false.");
+    this->config_.last_robot_state_fixed = false;
+  }
+
+  if(!this->private_node_handle_.getParam("all_robot_states_fixed", this->config_.all_robot_states_fixed))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'all_robot_states_fixed' not found. Setting it to false.");
+    this->config_.all_robot_states_fixed = false;
+  }
+
+  if(!this->private_node_handle_.getParam("amcl_localization", this->config_.amcl_localization))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'amcl_localization' not found. Setting it to false.");
+    this->config_.amcl_localization = false;
+  }
+
+  if(!this->private_node_handle_.getParam("calculate_covariance", this->config_.calculate_covariance))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'calculate_covariance' not found. Setting it to false.");
+    this->config_.calculate_covariance = false;
+  }
+
+  if(!this->private_node_handle_.getParam("publish_tf_map_odom", this->config_.publish_tf_map_odom))
+  {
+    ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'publish_tf_map_odom' not found. Setting it to false.");
+    this->config_.publish_tf_map_odom = false;
+  }
+
+  //Landmarks file
+  if (this->config_.load_landmarks)
+  {
+    if(!this->private_node_handle_.getParam("landmarks_map_file", this->config_.landmarks_map_file))
+    {
+      ROS_WARN("LandmarksTrackerAlgNode::LandmarksTrackerAlgNode: param 'landmarks_map_file' not found. Setting it to \"landmarks.txt\".");
+      this->config_.landmarks_map_file = "landmarks.txt";
+    }
+    load_landmarks();
+  }
+
   // [init publishers]
+  this->frame_data_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("frame_data", 1);
+  this->landmarks_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("landmarks", 1);
+  this->landmarks_localization_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("landmarks_localization_pose", 1);
   
   // [init subscribers]
+  this->estimated_pose_subscriber_ = this->private_node_handle_.subscribe("estimated_pose", 1, &AdcLandmarksSlamSolverAlgNode::estimated_pose_callback, this);
+  pthread_mutex_init(&this->estimated_pose_mutex_,NULL);
+
   this->features_subscriber_ = this->private_node_handle_.subscribe("features", 1, &AdcLandmarksSlamSolverAlgNode::features_callback, this);
   pthread_mutex_init(&this->features_mutex_,NULL);
 
+  //Init class atributes
   this->new_features_data = false;
+  this->new_estimated_pose = false;
+
+  this->init_tf_map_odom = this->config_.amcl_localization;
+  this->last_update_t = ros::Time::now();
+  this->last_update_odom << 0.0, 0.0, 0.0;
+  this->update_problem = false;
+
+  this->last_publish_t = ros::Time::now();
+  this->last_publish_odom << 0.0, 0.0, 0.0;
+  this->publish_pose = false;
+
   // [init services]
   
   // [init clients]
@@ -30,7 +135,10 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
 AdcLandmarksSlamSolverAlgNode::~AdcLandmarksSlamSolverAlgNode(void)
 {
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->estimated_pose_mutex_);
   pthread_mutex_destroy(&this->features_mutex_);
+  std::vector<Feature_info>().swap(this->features);
+  std::map<double, Landmark_info>().swap(this->landmarks);
 }
 
 void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
@@ -39,25 +147,237 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
   // this->alg_.lock();
   this->features_mutex_enter();
   // ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::mainNodeThread");
-  if (this->new_features_data)
+
+  if (this->init_tf_map_odom)
   {
-    std::cout << "new data" << std::endl;
-    this->new_features_data = false;
+    this->estimated_pose_mutex_enter();
+    if (this->new_estimated_pose)
+    {
+      this->new_estimated_pose = false;
+      tf2::Transform tf_map_odom, tf_odom_base, tf_map_base;
+      geometry_msgs::TransformStamped tf_odom_base_msg;
+      if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t, ros::Duration(this->config_.tf_timeout)))
+      {
+        ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:mainNodeThread -> Can't transform from " << this->config_.odom_frame << " to " << this->config_.base_link_frame << " at " << this->new_estimated_pose_t);
+        this->estimated_pose_mutex_exit();
+        this->features_mutex_exit();
+        return;
+      }
+      tf_odom_base_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t);
+      tf_odom_base.setOrigin(tf2::Vector3(tf_odom_base_msg.transform.translation.x, tf_odom_base_msg.transform.translation.y, tf_odom_base_msg.transform.translation.z));
+      tf_odom_base.setRotation(tf2::Quaternion(tf_odom_base_msg.transform.rotation.x, tf_odom_base_msg.transform.rotation.y, tf_odom_base_msg.transform.rotation.z, tf_odom_base_msg.transform.rotation.w));
+
+      tf_map_base.setOrigin(tf2::Vector3(this->estimated_pose(0), this->estimated_pose(1), 0.0));
+      tf2::Quaternion q;
+      q.setRPY(0.0, 0.0, this->estimated_pose(2));
+      tf_map_base.setRotation(q);
+
+      tf_map_odom = tf_map_base*tf_odom_base.inverse();
+      tf2::Vector3 origin;
+      origin = tf_map_odom.getOrigin();
+      this->tf_map_odom(0) = origin.getX();
+      this->tf_map_odom(1) = origin.getY();
+      double yaw, pitch, roll;
+      tf2::Matrix3x3(tf_map_odom.getRotation()).getEulerYPR(yaw, pitch, roll);
+      this->tf_map_odom(2) = yaw;
+    }
+    this->estimated_pose_mutex_exit();
+
+    if (this->new_features_data)
+    {
+      std::cout << "new data" << std::endl;
+      this->new_features_data = false;
+    }
+
+    // Publish fixed_frame to odom_frame tf
+    if (this->config_.publish_tf_map_odom && !this->config_.amcl_localization)
+    {
+      this->tf_map_odom_msg.header.stamp = ros::Time::now();
+      geometry_msgs::Transform tf;
+      tf.translation.x = this->tf_map_odom(0);
+      tf.translation.y = this->tf_map_odom(1);
+      tf.translation.z = 0.0;
+      tf2::Quaternion q;
+      q.setRPY(0.0, 0.0, this->tf_map_odom(2));
+      tf.rotation = tf2::toMsg(q);
+      this->tf_map_odom_msg.transform = tf;
+      this->tf2_broadcaster.sendTransform(this->tf_map_odom_msg);
+    }
   }
+  else
+  {
+    ros::Time t = ros::Time::now();
+    if (t - this->last_update_t > ros::Duration(1/this->config_.err_msg_rate))
+    {
+      ROS_ERROR("AdcLandmarksSlamSolverAlgNode::mainNodeThread -> tf_map_odom not initialized");
+      this->last_update_t = t;
+    }
+}
 
   // [fill msg structures]
+  // Initialize the topic message structure
+  //this->frame_data_MarkerArray_msg_.data = my_var;
+
+  // Initialize the topic message structure
+  //this->landmarks_MarkerArray_msg_.data = my_var;
+
+  // Initialize the topic message structure
+  //this->landmarks_localization_pose_PoseWithCovarianceStamped_msg_.data = my_var;
+
+  
+  /*
+  //tf2_listener example BEGIN
+  try{
+    std::string target_frame             = "child_frame";
+    std::string source_frame             = "parent_frame";
+    ros::Time time                       = ros::Time::now();
+    ros::Duration timeout                = ros::Duration(0.1);
+    this->alg_.unlock();
+    bool tf2_exists = this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout);
+    this->alg_.lock();
+    if(tf2_exists){
+      geometry_msgs::PoseStamped stamped_pose_in;
+      stamped_pose_in.header.stamp     = time;
+      stamped_pose_in.header.frame_id  = source_frame;
+      stamped_pose_in.pose.position.x    = 1.0;
+      stamped_pose_in.pose.position.y    = 0.0;
+      stamped_pose_in.pose.position.z    = 0.0;
+      tf2::Quaternion quat_tf;
+      quat_tf.setRPY(0.0, 0.0, 1.5709);
+      geometry_msgs::Quaternion quat_msg;
+      tf2::convert(quat_tf, quat_msg);
+      stamped_pose_in.pose.orientation = quat_msg;
+      double yaw, pitch, roll;
+      tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+      ROS_INFO("Original    pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_in.header.frame_id.c_str(), stamped_pose_in.pose.position.x, stamped_pose_in.pose.position.y, stamped_pose_in.pose.position.z, yaw, pitch, roll);
+      geometry_msgs::PoseStamped stamped_pose_out;
+      geometry_msgs::TransformStamped transform;
+      transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
+      quat_msg = transform.transform.rotation;
+      tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+      ROS_INFO("Found transform betwen frames (%s-->%s) with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", source_frame.c_str(), target_frame.c_str(), transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z, yaw, pitch, roll);
+      tf2::doTransform(stamped_pose_in, stamped_pose_out, transform);
+      quat_msg = stamped_pose_out.pose.orientation;
+      tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+      ROS_INFO("Transformed pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_out.header.frame_id.c_str(), stamped_pose_out.pose.position.x, stamped_pose_out.pose.position.y, stamped_pose_out.pose.position.z, yaw, pitch, roll);
+      ROS_INFO("---");
+    }else{
+      ROS_WARN("No transform found from '%s' to '%s'", source_frame.c_str(), target_frame.c_str()); }
+  }catch (tf2::TransformException &ex){
+    ROS_ERROR("TF2 Exception: %s",ex.what()); }
+  //tf2_listener example END
+  */
+
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  //this->frame_data_publisher_.publish(this->frame_data_MarkerArray_msg_);
+
+  // Uncomment the following line to publish the topic message
+  //this->landmarks_publisher_.publish(this->landmarks_MarkerArray_msg_);
+
+  // Uncomment the following line to publish the topic message
+  //this->landmarks_localization_pose_publisher_.publish(this->landmarks_localization_pose_PoseWithCovarianceStamped_msg_);
+
+  
+  /*
+  //tf2_broadcaster example BEGIN
+  this->transform_msg.header.stamp    = ros::Time::now();
+  this->transform_msg.header.frame_id = "parent_frame";
+  this->transform_msg.child_frame_id  = "child_frame";
+  geometry_msgs::Transform t;
+  t.translation.x = 0.0;
+  t.translation.y = 0.0;
+  t.translation.z = 0.0;
+  tf2::Quaternion q;
+  q.setRPY(0.0, 0.0, 0.0);
+  t.rotation = tf2::toMsg(q);
+  this->transform_msg.transform = t;
+  this->tf2_broadcaster.sendTransform(this->transform_msg);
+  //tf2_broadcaster example END
+  */
+
   
   // this->alg_.unlock();
   this->features_mutex_exit();
 }
 
+void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
+{
+  ROS_INFO_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> Loading from file " << this->config_.landmarks_map_file);
+  std::ifstream landmarks_file;
+  landmarks_file.open(this->config_.landmarks_map_file.c_str());
+  if (!landmarks_file.good())
+  {
+    ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
+    return;
+  }
+  std::map<double, Landmark_info>().swap(this->landmarks);
+  //Read the file
+  double id, l1, l2, l3;
+  int t;
+  while (landmarks_file >> id >> l1 >> l2 >> l3 >> t)
+  {
+    // std::cout << id << " " << l1 << " " << l2 << std::endl;
+    Landmark_info l;
+    l.detected = false;
+    l.pose(0) = l1;
+    l.pose(1) = l2;
+    l.pose(2) = l3;
+    l.type = t;
+    this->landmarks[id] = l;
+  }
+  // std::cout << "landmarks size = " << (int) this->global_landmarks.size() << std::endl;
+  landmarks_file.close();
+}
+
 /*  [subscriber callbacks] */
+void AdcLandmarksSlamSolverAlgNode::estimated_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
+{
+  ROS_INFO("AdcLandmarksSlamSolverAlgNode::estimated_pose_callback: New Message Received");
+
+  //use appropiate mutex to shared variables if necessary
+  //this->alg_.lock();
+  this->estimated_pose_mutex_enter();
+
+  if (!this->config_.amcl_localization)
+  {
+    this->new_estimated_pose = true;
+    this->init_tf_map_odom = true;
+    this->new_estimated_pose_t = msg->header.stamp;
+    this->estimated_pose(0) = msg->pose.pose.position.x;
+    this->estimated_pose(1) = msg->pose.pose.position.y;
+    this->estimated_sigma(0) = std::sqrt(msg->pose.covariance[0]);
+    this->estimated_sigma(1) = std::sqrt(msg->pose.covariance[7]);
+    this->estimated_sigma(2) = std::sqrt(msg->pose.covariance[35]);
+
+    tf2::Quaternion q;
+    q = tf2::Quaternion(0.0, 0.0, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
+    double yaw, pitch, roll;
+    tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+    this->estimated_pose(2) = yaw;
+  }
+
+  //std::cout << msg->data << std::endl;
+  //unlock previously blocked shared variables
+  //this->alg_.unlock();
+  this->estimated_pose_mutex_exit();
+}
+
+void AdcLandmarksSlamSolverAlgNode::estimated_pose_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->estimated_pose_mutex_);
+}
+
+void AdcLandmarksSlamSolverAlgNode::estimated_pose_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->estimated_pose_mutex_);
+}
+
 void AdcLandmarksSlamSolverAlgNode::features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg)
 {
   ROS_INFO("AdcLandmarksSlamSolverAlgNode::landmarks_callback: New Message Received");