From f69047e2f52a6e05b95f13c2d0aa55d53de98e4d Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sun, 21 May 2017 11:52:08 +0200
Subject: [PATCH] Replace Razor IMU with Tinkerforge IMU Brick v2

---
 config/wt_node.cfg          |  2 +-
 launch/wild_thumper.launch  |  4 +-
 scripts/tinkerforge_imu2.py | 79 +++++++++++++++++++++++++++++++++++++
 3 files changed, 81 insertions(+), 4 deletions(-)
 create mode 100755 scripts/tinkerforge_imu2.py

diff --git a/config/wt_node.cfg b/config/wt_node.cfg
index 4217be7..15fb37c 100755
--- a/config/wt_node.cfg
+++ b/config/wt_node.cfg
@@ -7,7 +7,7 @@ gen = ParameterGenerator()
 gen.add("range_sensor_clip",	bool_t,		0, "Clip range sensor values to max range", True)
 gen.add("range_sensor_max",	double_t,	0, "Range sensor max range", 0.5, 0.4, 4)
 gen.add("odom_covar_xy",	double_t,	0, "Odometry covariance: translation", 1e-2, 1e-6, 1)
-gen.add("odom_covar_angle",	double_t,	0, "Odometry covariance: rotation", 0.25, 1e-6, 6.2832)
+gen.add("odom_covar_angle",	double_t,	0, "Odometry covariance: rotation", 1.00, 1e-6, 6.2832)
 gen.add("rollover_protect",	bool_t,		0, "Enable motor rollover protection on pitch", True)
 gen.add("rollover_protect_limit",double_t,	0, "Pitch rollover protection limit (degree)", 45, 0, 90)
 gen.add("rollover_protect_pwm",	double_t,	0, "Pitch rollover protection speed (pwm)", 255, 0, 255)
diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index a21a14c..7fd4e59 100644
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -20,9 +20,7 @@
 		<param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
 	</node>
 
-	<node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen" if="$(arg use_imu)">
-		<rosparam file="$(find wild_thumper)/config/razor.yaml" command="load"/>
-	</node>
+	<node pkg="wild_thumper" type="tinkerforge_imu2.py" name="tinkerforge_imu_brick2" output="screen"/>
 
 	<node pkg="gpsd_client" type="gpsd_client"  name="gpsd_client" output="screen">
 		<param name="use_gps_time" value="false"/>
diff --git a/scripts/tinkerforge_imu2.py b/scripts/tinkerforge_imu2.py
new file mode 100755
index 0000000..fa00c26
--- /dev/null
+++ b/scripts/tinkerforge_imu2.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+HOST = "localhost"
+PORT = 4223
+UID = "6DdNSn"
+
+import rospy
+import prctl
+from sensor_msgs.msg import Imu
+from tinkerforge.ip_connection import IPConnection
+from tinkerforge.brick_imu_v2 import BrickIMUV2
+
+class ImuBrickv2:
+	def __init__(self):
+		rospy.init_node('imu_brick_v2')
+		prctl.set_name("imu_brick_v2")
+		self.pub_imu = rospy.Publisher("imu", Imu, queue_size=16)
+
+		ipcon = IPConnection() # Create IP connection
+		imu = BrickIMUV2(UID, ipcon) # Create device object
+		ipcon.connect(HOST, PORT) # Connect to brickd
+
+		imu.leds_off()
+		#imu.disable_status_led()
+
+		imu.register_callback(imu.CALLBACK_ALL_DATA, self.cb_all_data)
+		imu.set_all_data_period(20) # ms
+
+		rospy.spin()
+		ipcon.disconnect()
+
+
+	def cb_all_data(self, acceleration, magnetic_field, angular_velocity, euler_angle, quaternion, linear_acceleration, gravity_vector, temperature, calibration_status):
+		msg = Imu()
+		msg.header.stamp = rospy.Time.now()
+		msg.header.frame_id = "base_imu_link"
+
+		msg.orientation.x = quaternion[1]/16383.0
+		msg.orientation.y = quaternion[2]/16383.0
+		msg.orientation.z = quaternion[3]/16383.0
+		msg.orientation.w = quaternion[0]/16383.0
+		# Observed orientation variance: 0.0 (10k samples)
+		# Magnometer heading accuracy is +-2.5 deg => 0.088 rad
+		# With heading accuracy as std dev, variance = 0.088^2 = 0.008 
+		msg.orientation_covariance = [
+			0.008, 0     , 0,
+			0    , 0.008, 0,
+			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
+		# Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.01
+		msg.angular_velocity_covariance = [
+			0.01, 0   , 0,
+			0   , 0.01, 0,
+			0   , 0   , 0.01
+		]
+
+		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
+		# Observed linear acceleration variance: 0.001532 (10k samples)
+		# Calculation for variance raken from razor imu:
+		# nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
+		# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
+		msg.linear_acceleration_covariance = [
+			0.04, 0    , 0,
+			0    , 0.04, 0,
+			0    , 0    , 0.04
+		]
+
+		self.pub_imu.publish(msg)
+
+
+if __name__ == "__main__":
+	ImuBrickv2()	
-- 
2.39.2