From: Erik Andresen Date: Sun, 21 May 2017 09:52:08 +0000 (+0200) Subject: Replace Razor IMU with Tinkerforge IMU Brick v2 X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=f69047e2f52a6e05b95f13c2d0aa55d53de98e4d;p=ros_wild_thumper.git Replace Razor IMU with Tinkerforge IMU Brick v2 --- 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 @@ - - - + 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()