tinkerforge_imu2: Get settings from rosparam and fix angular_velocity &

linear_acceleration

diff --git a/scripts/tinkerforge_imu2.py b/scripts/tinkerforge_imu2.py
index 2cf42b2e710bb4f7d766030de7b89f46a9261925..59161c7e966f9b536d5f418e48c235e9d6106acf 100755 (executable)
--- a/scripts/tinkerforge_imu2.py
+++ b/scripts/tinkerforge_imu2.py
@@ -1,12 +1,9 @@
 #!/usr/bin/env python
 # -*- coding: iso-8859-15 -*-
 
-HOST = "localhost"
-PORT = 4223
-UID = "6DdNSn"
-
 import rospy
 import prctl
+from math import *
 from sensor_msgs.msg import Imu
 from tinkerforge.ip_connection import IPConnection
 from tinkerforge.brick_imu_v2 import BrickIMUV2
@@ -17,9 +14,13 @@ class ImuBrickv2:
                prctl.set_name("imu_brick_v2")
                self.pub_imu = rospy.Publisher("imu", Imu, queue_size=16)
 
+               host = rospy.get_param('~host', 'localhost')
+               port = rospy.get_param('~port', 4223)
+               uid = rospy.get_param('~uid')
+
                ipcon = IPConnection() # Create IP connection
-               imu = BrickIMUV2(UID, ipcon) # Create device object
-               ipcon.connect(HOST, PORT) # Connect to brickd
+               imu = BrickIMUV2(uid, ipcon) # Create device object
+               ipcon.connect(host, port) # Connect to brickd
 
                imu.leds_off()
                #imu.disable_status_led()
@@ -49,9 +50,9 @@ class ImuBrickv2:
                        0    , 0    , 0.008
                ]
 
-               msg.angular_velocity.x = angular_velocity[0]/16.0
-               msg.angular_velocity.y = angular_velocity[1]/16.0
-               msg.angular_velocity.z = angular_velocity[2]/16.0
+               msg.angular_velocity.x = angular_velocity[0]/16.0*pi/180
+               msg.angular_velocity.y = angular_velocity[1]/16.0*pi/180
+               msg.angular_velocity.z = angular_velocity[2]/16.0*pi/180
                # Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
                msg.angular_velocity_covariance = [
                        0.02, 0   , 0,
@@ -59,9 +60,9 @@ class ImuBrickv2:
                        0   , 0   , 0.02
                ]
 
-               msg.linear_acceleration.x = linear_acceleration[0]/100.0
-               msg.linear_acceleration.y = linear_acceleration[1]/100.0
-               msg.linear_acceleration.z = linear_acceleration[2]/100.0
+               msg.linear_acceleration.x = acceleration[0]/100.0
+               msg.linear_acceleration.y = acceleration[1]/100.0
+               msg.linear_acceleration.z = acceleration[2]/100.0
                # Observed linear acceleration variance: 0.001532 (10k samples)
                # Calculation for variance taken from razor imu:
                # nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2