From 21e7c7c3efef6e061cec75a966579ae14649d27d Mon Sep 17 00:00:00 2001
From: Mikhail Kurenkov <kurenkov@1995@mail.ru>
Date: Tue, 8 May 2018 02:11:33 +0300
Subject: [PATCH] k_bad = 0 for small robot

---
 .../particle_filter_node/particle_filter_node.py |  9 +++++++--
 eurobot_tests/src/get_pf_coords.py               | 16 +++++++++-------
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/eurobot/scripts/particle_filter_node/particle_filter_node.py b/eurobot/scripts/particle_filter_node/particle_filter_node.py
index dad7665..04df76d 100755
--- a/eurobot/scripts/particle_filter_node/particle_filter_node.py
+++ b/eurobot/scripts/particle_filter_node/particle_filter_node.py
@@ -16,7 +16,6 @@
              "min_intens": 3000,
              "max_dist": 3700,
              "k_angle": 120,
-             "k_bad": 2,
              "beac_dist_thresh": 150,
              "num_is_near_thresh": 0.1,
              "distance_noise_1_beacon": 1,
@@ -47,7 +46,13 @@ def __init__(self):
         lidar_odom_point = cvt_local2global(self.lidar_point, robot_odom_point)
         self.prev_lidar_odom_point = lidar_odom_point
         x, y, a = lidar_odom_point
-        self.pf = ParticleFilter(color=self.color, start_x=x, start_y=y, start_angle=a, **PF_PARAMS)
+
+        if self.robot_name == "secondary_robot":
+            k_bad = 0
+        else:
+            k_bad = 2
+
+        self.pf = ParticleFilter(color=self.color, start_x=x, start_y=y, start_angle=a, k_bad=k_bad, **PF_PARAMS)
         self.last_odom = np.zeros(3)
         self.alpha = 1
 
diff --git a/eurobot_tests/src/get_pf_coords.py b/eurobot_tests/src/get_pf_coords.py
index 3cf3cb9..fec6c7d 100755
--- a/eurobot_tests/src/get_pf_coords.py
+++ b/eurobot_tests/src/get_pf_coords.py
@@ -1,16 +1,16 @@
 #!/usr/bin/env python
 import rospy
-from sensor_msgs.msg import LaserScan
 import numpy as np
-import tf.transformations
-import tf
+import tf2_ros
+import tf_conversions
 import sys
 
 
 def get_coords():
-    (trans, rot) = listener.lookupTransform('map', robot_name, rospy.Time(0))
-    yaw = tf.transformations.euler_from_quaternion(rot)[2]
-    return np.array([trans[0], trans[1], yaw])
+    t = buffer1.lookup_transform('map', robot_name, rospy.Time(0))
+    q = [t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w]
+    yaw = tf_conversions.transformations.euler_from_quaternion(q)[2]
+    return True, np.array([t.transform.translation.x * 1000, t.transform.translation.y * 1000, yaw])
 
 
 def calculate_main(points):
@@ -27,11 +27,13 @@ def calculate_main(points):
     lidar_data = None
     rospy.init_node("get_coords")
     robot_name = sys.argv[2]
+    print(robot_name)
 
     rospy.loginfo("start get data")
     coords_full = []
     rate = rospy.Rate(20)
-    listener = tf.TransformListener()
+    buffer1 = tf2_ros.Buffer()
+    listener = tf2_ros.TransformListener(buffer1)
     time = int(sys.argv[1])
     for i in range(time * 20):
         rate.sleep()