distance_pub.py 1.43 KB
Newer Older
Johannes Bier's avatar
Johannes Bier committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
#!/bin/bash

exec python$ROS_PYTHON_VERSION - << EOF
import rospy
import tf
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Float64
from math import pi, atan2, sqrt

rospy.init_node('distance_sim', log_level=rospy.DEBUG)

distance_pub = rospy.Publisher("distance", Float64, queue_size=1)
tf_listener = tf.TransformListener()
wheel_left = None
wheel_right = None
distance = 0

def get_angle(q):
    signum = 1 if q[1] > 0 else -1
    return signum * 2 * atan2(sqrt(q[0] ** 2 + q[1] ** 2 + q[2] ** 2), q[3])

rate = rospy.Rate(12) # Hz
while not rospy.is_shutdown():
    try:
        wheel_left_quat = tf_listener.lookupTransform("base_link", "wheel_front_left_link", rospy.Time(0))[1]
        wheel_right_quat = tf_listener.lookupTransform("base_link", "wheel_front_right_link", rospy.Time(0))[1]
        wheel_left_new = get_angle(wheel_left_quat)
        wheel_right_new = get_angle(wheel_right_quat)
    except Exception as e:
        print("Failed to get transforms: ")
        print(e)
        rate.sleep()
        continue
    if wheel_left is not None:
        d_left = (((wheel_left_new - wheel_left) + pi) % (2*pi) - pi) * 0.085
        print(wheel_left_new, d_left)
        d_right = (((wheel_right_new - wheel_right) + pi) % (2*pi) - pi) * 0.085
        distance += (d_left + d_right) / 2
    wheel_left = wheel_left_new
    wheel_right = wheel_right_new
    distance_pub.publish(Float64(distance))
    rate.sleep()
EOF