resultados de la ejecucion de las pruebas

Desarrollo/Integracion ROS/localization_localino/launch/publish_anchors_tf.launch

@@ -4,7 +4,7 @@
   <!-- Output to screen -->
   <!-- <node name="anchors_static_tf_publisher" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="$(arg anchors_csv)" respawn="true" output="screen" /> -->
   <!-- Output to log -->
-  <node name="anchors_static_tf_publisher" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="/home/nico/Proyecto/RepoFinal/Investigacion/error_localizacion/posiciones1.csv" respawn="true" />
+  <node name="anchors_static_tf_publisher" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="/home/nico/Proyecto/RepoFinal/Investigacion/pruebas.csv" respawn="true" />
 
   <!-- Original -->
   <!-- <node name="anchors" pkg="localization_localino" type="anchors_static_tf_publisher.py" args="/root/catkin_ws/src/localization_localino/scripts/anchors.csv" respawn="true"/> -->

Desarrollo/Integracion ROS/localization_localino/launch/run_multi_filters.launch

+<!-- lanch para ejecutar los 4 filtros a la vez y poder generar la data de la 
+diferencia entre la posicion calculada y la posicion estimada por el sistema -->
+
 <launch>
   	<node name="reader_0" pkg="localization_localino" type="reader.py">
 <rosparam>

Desarrollo/Integracion ROS/localization_localino/launch/run_rviz.launch

 <launch >
 <!-- <include file="$(find localization_localino)/launch/run.launch"/>  -->
-<include file="$(find localization_localino)/launch/run_test.launch"/> 
+<!-- <include file="$(find localization_localino)/launch/run_test.launch"/>  -->
 <include file="$(find localization_localino)/launch/publish_anchors_tf.launch"/>
  <node name="visualizar" pkg="rviz" type="rviz" respawn="true" args="-d $(find localization_localino)/rviz/nodos.rviz"/>
   <!-- <node name="fixed_frame" pkg="localization_localino" type="fixed_frame.py" respawn="true" args="-d $(find localization_localino)/rviz/nodos.rviz"/> -->

Desarrollo/Integracion ROS/localization_localino/launch/run_sim_error_disposicion.launch

+<!-- launch para calcular el error en la disposicion de los tags -->
+<launch>
+<node name="reader_0" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 0
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        anchor2: 10,
+        #anchor3: 1,
+        #anchor4: 16,
+        anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        anchor8: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        #anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        anchor2:  [-3.688,2.904,1.749],
+        #anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        anchor8:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        #anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_0
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_0" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_0
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/diferencias_disposiciones/en_linea_filtro_0.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="reader_1" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 1
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        anchor2: 10,
+        #anchor3: 1,
+        #anchor4: 16,
+        anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        anchor8: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        #anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        anchor2:  [-3.688,2.904,1.749],
+        #anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        anchor8:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        #anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_1
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_1" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_1
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/diferencias_disposiciones/en_linea_filtro_1.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="reader_2" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 2
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        anchor2: 10,
+        #anchor3: 1,
+        #anchor4: 16,
+        anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        anchor8: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        #anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        anchor2:  [-3.688,2.904,1.749],
+        #anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        anchor8:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        #anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_2
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_2" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_2
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/diferencias_disposiciones/en_linea_filtro_2.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="reader_3" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 3
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        anchor2: 10,
+        #anchor3: 1,
+        #anchor4: 16,
+        anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        anchor8: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        #anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        anchor2:  [-3.688,2.904,1.749],
+        #anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        anchor8:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        #anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_3
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_3" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_3
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/diferencias_disposiciones/en_linea_filtro_3.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="stop" required="true" pkg="localization_localino" type="stop_launch.py" output="screen">
+    <rosparam>
+        cant_procesos: 4
+    </rosparam>
+</node>
+<arg name="carpeta" value="/home/nico/Proyecto/RepoFinal/Investigacion/bags_distancias/" />
+
+<node pkg="rosbag" type="play" name="player13" output="screen" args="--wait-for-subscribers -q --clock $(arg carpeta)/pto_10.bag"/>
+<node pkg="rosbag" type="play" name="player16" output="screen" args="--wait-for-subscribers -q --clock $(arg carpeta)/pto_17.bag"/>
+<node pkg="rosbag" type="play" name="player17" output="screen" args="--wait-for-subscribers -q --clock $(arg carpeta)/pto_2.bag"/>
+<node pkg="rosbag" type="play" name="player18" output="screen" args="--wait-for-subscribers -q --clock $(arg carpeta)/pto_18.bag"/>
+
+</launch>

Desarrollo/Integracion ROS/localization_localino/launch/run_sim_error_multipath.launch

+<!-- launch para calcular el error en la disposicion de los tags -->
+<launch>
+<node name="reader_0" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 0
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        #anchor2: 10,
+        anchor3: 1,
+        #anchor4: 16,
+        #anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        #anchor3: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        #anchor2:  [-3.688,2.904,1.749],
+        anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        #anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        #anchor3:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_0
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_0" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_0
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/efecto_multipath/con_multipath_filtro_0.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="reader_1" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 1
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        #anchor2: 10,
+        anchor3: 1,
+        #anchor4: 16,
+        #anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        #anchor3: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        #anchor2:  [-3.688,2.904,1.749],
+        anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        #anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        #anchor3:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_1
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_1" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_1
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/efecto_multipath/con_multipath_filtro_1.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="reader_2" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 2
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        #anchor2: 10,
+        anchor3: 1,
+        #anchor4: 16,
+        #anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        #anchor3: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        #anchor2:  [-3.688,2.904,1.749],
+        anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        #anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        #anchor3:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_2
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_2" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_2
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/efecto_multipath/con_multipath_filtro_2.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="reader_3" pkg="localization_localino" type="reader.py">
+    <rosparam>
+        #0 sin filtro, 1 Afa beta, 2 Kalman, 3 Promedio
+        filter: 3
+        kalmanr: 1.5
+        kalmanq: 1000
+        kalmandt: 0.3
+        kalmansv: 0.5
+        #offset, diferencia entre la medida real y la calculada default -18.5
+        offset: 0.0
+        alpha: 0.5
+        beta: 0.5
+        #cantidad de valores a recordar
+        filterma: 4
+        anchors_ids: {
+        #anchor1: 5,
+        #anchor2: 10,
+        anchor3: 1,
+        #anchor4: 16,
+        #anchor5: 17,
+        #anchor6: 13,
+        anchor7: 2,
+        #anchor3: 18,
+        #anchor9: 7,
+        #anchor10: 4,
+        anchor11: 3,
+        #anchor12: 14,
+        #anchor13: 19,
+        }
+        anchor_pos: {
+        #anchor1:  [3.276,5.405,1.207],
+        #anchor2:  [-3.688,2.904,1.749],
+        anchor3:  [0.869,9.077,1.649],
+        #anchor4:  [3.083,-5.714,1.709],
+        #anchor5:  [-3.697,-3.163,1.725],
+        #anchor6:  [3.282,1.457,1.594],
+        anchor7:  [-3.692,8.882,1.624],
+        #anchor3:  [-3.701,-0.824,1.493],
+        #anchor9:   [3.295,2.778,1.803],
+        #anchor10:  [3.274,5.404,1.732],
+        anchor11:  [3.291,7.809,1.691],
+        #anchor12:  [3.28,-0.577,1.731],
+        #anchor13:  [-3.696,0.378,1.983],
+        }
+        #es la altura del tag, asumimos que el tag esta siempre a la misma altura
+        tagheight: 1.725
+        topic_debug: debug_position_3
+        debug: true
+    </rosparam>
+</node>
+<node name="position_processor_3" pkg="localization_localino" type="position-processor.py" output="screen">
+    <rosparam>
+        tag_real_pos: [0.091, 4.091, 1.725]
+        input_topic: debug_position_3
+        output_file: /home/nico/Proyecto/RepoFinal/Investigacion/efecto_multipath/con_multipath_filtro_3.csv
+        cant_medidas: 1000
+    </rosparam>
+</node>
+<node name="stop" required="true" pkg="localization_localino" type="stop_launch.py" output="screen">
+    <rosparam>
+        cant_procesos: 4
+    </rosparam>
+</node>
+<arg name="carpeta" value="/home/nico/Proyecto/RepoFinal/Investigacion/bags_distancias/" />
+
+<node pkg="rosbag" type="play" name="player13" output="screen" args="--wait-for-subscribers -q --clock $(arg carpeta)/rebote.bag"/>
+
+</launch>

Investigacion/bags_distancias/leer.txt

@@ -27,7 +27,7 @@ ptos obstruidos por una madera de 2.0 m de alto, 0.60 m de ancho y 18 mm
 de espesor a 2 m del anchor en direccion al tag,
 nombre de los bags pto_X_O.bag ptos 1 , 2, 3, 14, 18
 
-bad de rebote, tablon a 1m del tag, 
+bag de rebote, tablon a 1m del tag, 
 puntos 1, 2 ,3, nombre, rebote.bag
 se tomaron 4000 msg