Added config files and updated launch files

config/amcl_mapping.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "odom"
+base_link_frame: "base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+visualization : True
+load_landmarks : False
+landmarks_pos_fixed : False
+search_for_new_landmarks : True
+first_robot_state_fixed : False
+last_robot_state_fixed : True
+all_robot_states_fixed : True
+amcl_localization : True
+calculate_covariance : False
+publish_tf_map_odom : False
+
+update_problem_rate: 0.1
+update_problem_distance: 0.3
+update_problem_angle: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 5.0
+landmarks_min_detections: 15
+
+sensor_sigma_r: 0.15
+sensor_sigma_th: 0.1
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/landmarks_calibration.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "odom"
+base_link_frame: "base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+visualization : True
+load_landmarks : True
+landmarks_pos_fixed : False
+search_for_new_landmarks : False
+first_robot_state_fixed : True
+last_robot_state_fixed : False
+all_robot_states_fixed : False
+amcl_localization : False
+calculate_covariance : False
+publish_tf_map_odom : True
+
+update_problem_rate: 0.1
+update_problem_distance: 0.3
+update_problem_angle: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 5.0
+landmarks_min_detections: 15
+
+sensor_sigma_r: 0.15
+sensor_sigma_th: 0.1
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/localization.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "odom"
+base_link_frame: "base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+visualization : True
+load_landmarks : True
+landmarks_pos_fixed : True
+search_for_new_landmarks : False
+first_robot_state_fixed : False
+last_robot_state_fixed : False
+all_robot_states_fixed : False
+amcl_localization : False
+calculate_covariance : True
+publish_tf_map_odom : False
+
+update_problem_rate: 0.1
+update_problem_distance: 0.3
+update_problem_angle: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 5.0
+landmarks_min_detections: 15
+
+sensor_sigma_r: 0.15
+sensor_sigma_th: 0.1
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

config/slam_mapping.yaml

+rate: 200.0
+global_frame: "map"
+odom_frame: "odom"
+base_link_frame: "base_link"
+tf_timeout: 0.1
+publish_pose_rate: 0.001
+publish_pose_distance: 0.1
+publish_pose_angle: 0.1
+
+visualization : True
+load_landmarks : False
+landmarks_pos_fixed : False
+search_for_new_landmarks : True
+first_robot_state_fixed : True
+last_robot_state_fixed : False
+all_robot_states_fixed : False
+amcl_localization : False
+calculate_covariance : False
+publish_tf_map_odom : True
+
+update_problem_rate: 0.1
+update_problem_distance: 0.3
+update_problem_angle: 0.2
+problem_frame_window: -1
+
+landmarks_candidates_filter_en: True
+landmark_mahalanobis_dist: 5.0
+landmarks_min_detections: 15
+
+sensor_sigma_r: 0.15
+sensor_sigma_th: 0.1
+
+odom_fxy: 0.1
+odom_fth: 0.15
+odom_fxyth: 0.25
+odom_sigma_min: 0.000001

launch/node.launch

 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
-  <arg name="node_name"     default="iri_adc_landmarks_slam_solver"/>
-  <arg name="output"        default="screen"/>
-  <arg name="launch_prefix" default=""/>
-  <arg name="config_file"   default="$(find iri_adc_landmarks_slam_solver)/config/params.yaml"/>
+  <arg name="node_name"            default="iri_adc_landmarks_slam_solver"/>
+  <arg name="output"               default="screen"/>
+  <arg name="launch_prefix"        default=""/>
+  <arg name="config_file"          default="$(find iri_adc_landmarks_slam_solver)/config/params.yaml"/>
+  <arg name="landmarks_map_file"   default="$(find iri_adc_landmarks_slam_solver)/data/landmarks.txt"/>
   <!-- <arg name="topic_name"  default="new_topic_name"/> -->
+  <arg name="estimated_pose_topic_name" default="~/estimated_pose"/>
+  <arg name="features_topic_name" default="~/features"/>
 
   <node name="$(arg node_name)"
         pkg ="iri_adc_landmarks_slam_solver"
@@ -13,7 +16,10 @@
         output="$(arg output)"
         launch-prefix="$(arg launch_prefix)">
     <rosparam file="$(arg config_file)" command="load"/>
+    <param name="landmarks_map_file" value="$(arg landmarks_map_file)"/>
     <!--<remap from="~/topic" to="$(arg topic_name)"/>-->
+    <remap from="~/estimated_pose" to="$(arg estimated_pose_topic_name)"/>
+    <remap from="~/features" to="$(arg features_topic_name)"/>
   </node>
 
-</launch>
+</launch>
\ No newline at end of file

launch/test.launch

 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
 
-  <arg name="output"        default="screen"/>
-  <arg name="launch_prefix" default=""/>
-  <arg name="dr"            default="true"/>
+  <arg name="output"                default="screen"/>
+  <arg name="launch_prefix"         default=""/>
+  <arg name="dr"                    default="true"/>
+  <arg name="config_dir"            default="$(find iri_adc_landmarks_slam_solver)/config/"/>
+  <arg name="config_file"           default="localization.yaml"/>
+  <arg name="data_dir"              default="$(find iri_adc_landmarks_slam_solver)/data/"/>
+  <arg name="landmarks_file"        default="landmarks.txt"/>
+
+  <arg name="estimated_pose_topic_name" default="/initialpose"/>
+  <arg name="features_topic_name" default="/iri_adc_tag_localization_filter/features"/>
 
   <include file="$(find iri_adc_landmarks_slam_solver)/launch/node.launch">
     <arg name="node_name"     value="iri_adc_landmarks_slam_solver"/>
     <arg name="output"        value="$(arg output)"/>
     <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    <arg name="config_file"           value="$(arg config_dir)/$(arg config_file)"/>
+    <arg name="landmarks_map_file"    value="$(arg data_dir)/$(arg landmarks_file)"/>
+    <arg name="estimated_pose_topic_name"  value="$(arg estimated_pose_topic_name)"/>
+    <arg name="features_topic_name"  value="$(arg features_topic_name)"/>
   </include>
 
   <node name="rqt_reconfigure_iri_adc_landmarks_slam_solver"
@@ -18,4 +29,4 @@
         args="iri_adc_landmarks_slam_solver">
   </node>
 
-</launch>
+</launch>
\ No newline at end of file

src/adc_landmarks_slam_solver_alg_node.cpp

@@ -338,7 +338,7 @@ void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
 /*  [subscriber callbacks] */
 void AdcLandmarksSlamSolverAlgNode::estimated_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
 {
-  ROS_INFO("AdcLandmarksSlamSolverAlgNode::estimated_pose_callback: New Message Received");
+  ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::estimated_pose_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
@@ -380,7 +380,7 @@ void AdcLandmarksSlamSolverAlgNode::estimated_pose_mutex_exit(void)
 
 void AdcLandmarksSlamSolverAlgNode::features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg)
 {
-  ROS_INFO("AdcLandmarksSlamSolverAlgNode::landmarks_callback: New Message Received");
+  ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::landmarks_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();