init code push

CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rasp_imu_hat_6dof)
+
+find_package(catkin REQUIRED COMPONENTS 
+    rospy
+    dynamic_reconfigure)
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+generate_dynamic_reconfigure_options(
+    cfg/imu.cfg
+)
+
+catkin_package()
+
+include_directories(
+    ${catkin_INCLUDE_DIRS}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY cfg
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY src
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+catkin_install_python(PROGRAMS 
+	nodes/imu_node.py
+	nodes/display_3D_visualization.py
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
+)

cfg/imu.cfg

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+PACKAGE = "rasp_imu_hat_6dof"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("yaw_calibration", double_t, 0, "Yaw Calibration", 0, -180, 180)
+
+exit(gen.generate(PACKAGE, "rasp_imu_hat_6dof", "imu"))

config/param.yaml

@@ -0,0 +1,3 @@
+pub_topic: imu
+link_name: base_imu_link
+pub_hz: 10

launch/imu-display.launch

@@ -0,0 +1,4 @@
+<launch>
+  <node pkg="rasp_imu_hat_6dof" type="display_3D_visualization.py" name="display_3D_visualization_node" output="screen">
+  </node>
+</launch>

launch/imu-pub-and-display.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="imu_config_file" default="$(find rasp_imu_hat_6dof)/config/param.yaml"/>
+  <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
+    <rosparam file="$(arg imu_config_file)" command="load"/>
+  </node>
+
+  <node pkg="rasp_imu_hat_6dof" type="display_3D_visualization.py" name="display_3D_visualization_node" output="screen">
+  </node>
+</launch>

launch/imu-pub.launch

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="imu_config_file" default="$(find rasp_imu_hat_6dof)/config/param.yaml"/>
+  <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
+    <rosparam file="$(arg imu_config_file)" command="load"/>
+  </node>
+</launch>
+

nodes/display_3D_visualization.py

@@ -0,0 +1,144 @@
+#!/usr/bin/env python
+
+import rospy
+from visual import *
+import math
+import wx
+
+from sensor_msgs.msg import Imu
+from tf.transformations import euler_from_quaternion
+
+rad2degrees = 180.0/math.pi
+precision = 2 #round to this number of digits
+yaw_offset = 0 #used to align animation upon key press
+
+
+#Create shutdown hook to kill visual displays
+def shutdown_hook():
+    #print "Killing displays"
+    wx.Exit()
+
+#register shutdown hook
+rospy.on_shutdown(shutdown_hook)
+
+
+# Main scene
+scene=display(title="9DOF Razor IMU Main Screen")
+scene.range=(1.3,1.3,1.3)
+scene.forward = (1,0,-0.25)
+# Change reference axis (x,y,z) - z is up
+scene.up=(0,0,1)
+scene.width=500
+scene.height=500
+
+# Second scene (Roll, Pitch, Yaw)
+scene2 = display(title='6DOF IMU Roll, Pitch, Yaw',x=550, y=0, width=500, height=500,center=(0,0,0), background=(0,0,0))
+scene2.range=(1,1,1)
+scene2.select()
+#Roll, Pitch, Yaw
+#Default reference, i.e. x runs right, y runs up, z runs backward (out of screen)
+cil_roll = cylinder(pos=(-0.5,0.3,0),axis=(0.2,0,0),radius=0.01,color=color.red)
+cil_roll2 = cylinder(pos=(-0.5,0.3,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
+cil_pitch = arrow(pos=(0.5,0.3,0),axis=(0,0,-0.4),shaftwidth=0.02,color=color.green)
+arrow_course = arrow(pos=(0.0,-0.4,0),color=color.cyan,axis=(0,0.2,0), shaftwidth=0.02, fixedwidth=1)
+
+#Roll,Pitch,Yaw labels
+label(pos=(-0.5,0.6,0),text="Roll (degrees / radians)",box=0,opacity=0)
+label(pos=(0.5,0.6,0),text="Pitch (degrees / radians)",box=0,opacity=0)
+label(pos=(0.0,0.02,0),text="Yaw (degrees / radians)",box=0,opacity=0)
+label(pos=(0.0,-0.16,0),text="E",box=0,opacity=0,color=color.yellow)
+label(pos=(0.0,-0.64,0),text="W",box=0,opacity=0,color=color.yellow)
+label(pos=(-0.24,-0.4,0),text="N",box=0,opacity=0,color=color.yellow)
+label(pos=(0.24,-0.4,0),text="S",box=0,opacity=0,color=color.yellow)
+label(pos=(0.18,-0.22,0),height=7,text="SE",box=0,color=color.yellow)
+label(pos=(-0.18,-0.22,0),height=7,text="NE",box=0,color=color.yellow)
+label(pos=(0.18,-0.58,0),height=7,text="SW",box=0,color=color.yellow)
+label(pos=(-0.18,-0.58,0),height=7,text="NW",box=0,color=color.yellow)
+
+rollLabel = label(pos=(-0.5,0.52,0),text="-",box=0,opacity=0,height=12)
+pitchLabel = label(pos=(0.5,0.52,0),text="-",box=0,opacity=0,height=12)
+yawLabel = label(pos=(0,-0.06,0),text="-",box=0,opacity=0,height=12)
+
+#acceleration labels
+label(pos=(0,0.9,0),text="Linear Acceleration x / y / z (m/s^2)",box=0,opacity=0)
+label(pos=(0,-0.8,0),text="Angular Velocity x / y / z (rad/s)",box=0,opacity=0)
+linAccLabel = label(pos=(0,0.82,0),text="-",box=0,opacity=0,height=12)
+angVelLabel = label(pos=(0,-0.88,0),text="-",box=0,opacity=0,height=12)
+
+# Main scene objects
+scene.select()
+# Reference axis (x,y,z) - using ROS conventions (REP 103) - z is up, y left (north, 90 deg), x is forward (east, 0 deg)
+# In visual, z runs up, x runs forward, y runs left (see scene.up command earlier)
+# So everything is good
+arrow(color=color.green,axis=(1,0,0), shaftwidth=0.04, fixedwidth=1)
+arrow(color=color.green,axis=(0,1,0), shaftwidth=0.04 , fixedwidth=1)
+arrow(color=color.green,axis=(0,0,1), shaftwidth=0.04, fixedwidth=1)
+
+# labels
+label(pos=(0,0,-1.2),text="Press 'a' to align",box=0,opacity=0)
+label(pos=(1,0.1,0),text="X",box=0,opacity=0)
+label(pos=(0,1,-0.1),text="Y",box=0,opacity=0)
+label(pos=(0,-0.1,1),text="Z",box=0,opacity=0)
+# IMU object
+platform = box(length=1.0, height=0.05, width=0.65, color=color.red,up=(0,0,1),axis=(-1,0,0))
+p_line = box(length=1.1,height=0.08,width=0.1,color=color.yellow,up=(0,0,1),axis=(-1,0,0))
+plat_arrow = arrow(length=-0.8,color=color.cyan,up=(0,0,1),axis=(-1,0,0), shaftwidth=0.06, fixedwidth=1)
+plat_arrow_up = arrow(length=0.4,color=color.cyan,up=(-1,0,0),axis=(0,0,1), shaftwidth=0.06, fixedwidth=1)
+rospy.init_node("display_3D_visualization_node")
+
+def processIMU_message(imuMsg):
+    global yaw_offset
+
+    roll=0
+    pitch=0
+    yaw=0
+
+    quaternion = (
+      imuMsg.orientation.x,
+      imuMsg.orientation.y,
+      imuMsg.orientation.z,
+      imuMsg.orientation.w)
+    (roll,pitch,yaw) = euler_from_quaternion(quaternion)
+
+    #add align offset to yaw
+    yaw += yaw_offset
+
+    axis=(-cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))
+    up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),-sin(roll)*cos(yaw)+cos(roll)*sin(pitch)*sin(yaw),cos(roll)*cos(pitch))
+    platform.axis=axis
+    platform.up=up
+    platform.length=1.0
+    plat_arrow_up.axis=up
+    plat_arrow_up.up=axis
+    plat_arrow_up.length=0.4
+    plat_arrow.axis=axis
+    plat_arrow.up=up
+    plat_arrow.length=-0.8
+    p_line.axis=axis
+    p_line.up=up
+    p_line.length=1.1
+    cil_roll.axis=(-0.2*cos(roll),0.2*sin(roll),0)
+    cil_roll2.axis=(0.2*cos(roll),-0.2*sin(roll),0)
+    cil_pitch.axis=(0,-0.4*sin(pitch),-0.4*cos(pitch))
+    #remove yaw_offset from yaw display
+    arrow_course.axis=(-0.2*sin(yaw-yaw_offset),0.2*cos(yaw-yaw_offset),0)
+
+    #display in degrees / radians
+    rollLabel.text = str(round(roll*rad2degrees, precision)) + " / " + str(round(roll,precision))
+    pitchLabel.text = str(round(pitch*rad2degrees, precision)) + " / " + str(round(pitch, precision))
+    #remove yaw_offset from yaw display
+    yawLabel.text = str(round((yaw-yaw_offset)*rad2degrees, precision)) + " / " + str(round((yaw-yaw_offset), precision))
+
+    linAccLabel.text = str(round(imuMsg.linear_acceleration.x, precision)) + " / " + str(round(imuMsg.linear_acceleration.y, precision)) + " / " + str(round(imuMsg.linear_acceleration.z, precision))
+    angVelLabel.text = str(round(imuMsg.angular_velocity.x, precision)) + " / " + str(round(imuMsg.angular_velocity.y, precision)) + " / " + str(round(imuMsg.angular_velocity.z, precision))
+
+    #check for align key press - if pressed, next refresh will be aligned
+    if scene.kb.keys: # event waiting to be processed?
+        s = scene.kb.getkey() # get keyboard info
+        if s == 'a':
+            #align key pressed - align
+            yaw_offset += -yaw
+
+sub = rospy.Subscriber('imu', Imu, processIMU_message)
+rospy.spin()
+

nodes/imu_node.py

@@ -0,0 +1,162 @@
+#!/usr/bin/env python
+
+import smbus
+import rospy
+import string
+import math
+import time
+import sys
+
+from tf.transformations import quaternion_from_euler
+from dynamic_reconfigure.server import Server
+from rasp_imu_hat_6dof.cfg import imuConfig
+from sensor_msgs.msg import Imu
+
+
+degrees2rad = math.pi/180.0
+imu_yaw_calibration = 0.0
+
+# Callback for dynamic reconfigure requests
+def reconfig_callback(config, level):
+    global imu_yaw_calibration
+    imu_yaw_calibration = config['yaw_calibration']
+    rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
+    return config
+
+
+rospy.init_node("imu_node")
+
+#Get DIY param
+topic_name = rospy.get_param('~pub_topic', 'imu')
+link_name  = rospy.get_param('~link_name', 'base_imu_link')
+pub_hz     = rospy.get_param('~pub_hz', '10')
+
+
+pub = rospy.Publisher(topic_name, Imu, queue_size=1)
+srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
+imuMsg = Imu()
+
+# Orientation covariance estimation:
+# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
+# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
+# static roll/pitch error of 0.8%, owing to gravity orientation sensing
+# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
+# so set all covariances the same.
+imuMsg.orientation_covariance = [
+0.0025 , 0 , 0,
+0, 0.0025, 0,
+0, 0, 0.0025
+]
+
+# Angular velocity covariance estimation:
+# Observed gyro noise: 4 counts => 0.28 degrees/sec
+# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
+# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
+imuMsg.angular_velocity_covariance = [
+0.02, 0 , 0,
+0 , 0.02, 0,
+0 , 0 , 0.02
+]
+
+# linear acceleration covariance estimation:
+# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
+# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
+# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
+imuMsg.linear_acceleration_covariance = [
+0.04 , 0 , 0,
+0 , 0.04, 0,
+0 , 0 , 0.04
+]
+
+seq=0
+accel_factor = 9.806/256.0    # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
+
+class MyIMU(object):
+    def __init__(self, addr):
+        self.addr = addr
+        self.i2c = smbus.SMBus(1)
+
+    def get_YPRAG(self):
+        try:
+            roll_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x3d, 2)
+            pitch_tmp = self.i2c.read_i2c_block_data(self.addr, 0x3e, 2)
+            yaw_tmp   = self.i2c.read_i2c_block_data(self.addr, 0x3f, 2)
+
+            ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
+            ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
+            az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
+
+            gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
+            gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
+            gz_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x39, 2)
+        except IOError:
+            rospy.logerr("Read IMU YPRAG date error !")
+        else:
+            self.raw_roll  = float(((roll_tmp[1]<<8) |roll_tmp[0])/32768.0*180.0)
+            self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
+            self.raw_yaw  = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
+
+            self.raw_ax  = float(((ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
+            self.raw_ay  = float(((ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
+            self.raw_az  = float(((az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
+
+            self.raw_gx  = float(((gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
+            self.raw_gy  = float(((gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
+            self.raw_gz  = float(((gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
+
+    def get_quatern(self):
+        try:
+            q0 = self.i2c.read_i2c_block_data(self.addr, 0x51, 2)
+            q1 = self.i2c.read_i2c_block_data(self.addr, 0x52, 2)
+            q2 = self.i2c.read_i2c_block_data(self.addr, 0x53, 2)
+            q3 = self.i2c.read_i2c_block_data(self.addr, 0x54, 2)
+        except IOError:
+            rospy.logerr("Read IMU quaternion date error !")
+        else:
+            self.raw_q0 = float(((q0[1]<<8)|q0[0])/32768.0)
+            self.raw_q1 = float(((q1[1]<<8)|q1[0])/32768.0)
+            self.raw_q2 = float(((q2[1]<<8)|q2[0])/32768.0)
+            self.raw_q3 = float(((q3[1]<<8)|q3[0])/32768.0)
+
+
+myIMU = MyIMU(0x50)
+rate = rospy.Rate(pub_hz)
+
+while not rospy.is_shutdown():
+    myIMU.get_YPRAG()
+
+    yaw_deg = float(myIMU.raw_yaw)
+    yaw_deg = yaw_deg + imu_yaw_calibration
+    if yaw_deg >= 180.0:
+        yaw_deg -= 360.0
+
+    #rospy.loginfo("yaw_deg: %f", yaw_deg)
+    yaw = yaw_deg*degrees2rad
+
+    #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103)
+    pitch = float(myIMU.raw_pitch)*degrees2rad
+    roll  = float(myIMU.raw_roll)*degrees2rad
+    #rospy.loginfo("yaw:%f pitch:%f  rool:%f", yaw, pitch, roll)
+
+    # Publish message
+    imuMsg.linear_acceleration.x = float(myIMU.raw_ax)*accel_factor
+    imuMsg.linear_acceleration.y = float(myIMU.raw_ay)*accel_factor
+    imuMsg.linear_acceleration.z = float(myIMU.raw_az)*accel_factor
+
+    imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
+    imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
+    imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad
+
+    #myIMU.get_quatern()
+    q = quaternion_from_euler(roll, pitch, yaw)
+    imuMsg.orientation.x = q[0]
+    imuMsg.orientation.y = q[1]
+    imuMsg.orientation.z = q[2]
+    imuMsg.orientation.w = q[3]
+    imuMsg.header.stamp= rospy.Time.now()
+    imuMsg.header.frame_id = link_name
+    imuMsg.header.seq = seq
+    pub.publish(imuMsg)
+    seq = seq + 1
+    rate.sleep()
+

package.xml

@@ -0,0 +1,25 @@
+<package>
+  <name>rasp_imu_hat_6dof</name>
+  <version>1.2.0</version>
+  <description>
+     rasp_imu_hat_6dof is a package that provides a ROS driver
+      for the corvin 6dof imu hat.
+  </description>
+
+  <maintainer email="corvin_zhang@corvin.cn">Corvin Zhang</maintainer>
+
+  <license>BSD</license>
+  <author>Corvin Zhang</author>
+
+  <url type="website">https://www.corvin.cn</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>dynamic_reconfigure</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+</package>
+