From f27acff1540cb862a0951359633f0a5fa1a51fda Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Wed, 14 Jun 2017 07:20:06 +0200 Subject: [PATCH] tinkerforge_imu2: Get settings from rosparam and fix angular_velocity & linear_acceleration --- scripts/tinkerforge_imu2.py | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) 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 -- 2.39.5