From: Erik Andresen <erik@vontaene.de>
Date: Wed, 14 Jun 2017 05:20:06 +0000 (+0200)
Subject: tinkerforge_imu2: Get settings from rosparam and fix angular_velocity &
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=f27acff1540cb862a0951359633f0a5fa1a51fda;p=ros_wild_thumper.git
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 2cf42b2..59161c7 100755
--- 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