Publishing the imu of the steam controller

pub_imu_steam.py

#!/usr/bin/env python

"""Publish IMU msg of steam controller"""

from __future__ import division
import sys
from steamcontroller import SteamController
import struct
import rospy
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion, Vector3, PoseStamped, WrenchStamped


MAX_VAL = 32767
MIN_VAL = -32768


def normalize(val, min_val, max_val):
    val_between_0_and_1 = (val - min_val) / (abs(min_val) + abs(max_val))
    val_between_minus_1_and_1 = (val_between_0_and_1 * 2.0) - 1.0
    return val_between_minus_1_and_1


def fix(val):
    return normalize(val, MIN_VAL, MAX_VAL)


def create_quaternion(q1, q2, q3, q4):
    print "Creating quaternion from:"
    print (q1, q2, q3, q4)
    q = Quaternion()
    # Failed attempt
    # q.x = fix(q1)
    # q.y = fix(q2)
    # q.z = fix(q3)
    # q.w = fix(q4)
    # The quaternion IS w,x,y,z
    q.x = fix(q2)
    q.y = fix(q3)
    q.z = fix(q4)
    q.w = fix(q1)
    return q


def create_vector3(groll, gpitch, gyaw):
    print "Creating Vector3 from:"
    print (groll, gpitch, gyaw)
    v = Vector3()
    # I guess the units will be like that, but I don't know
    v.x = groll / 1000.0
    v.y = gpitch / 1000.0
    v.z = gyaw / 1000.0
    return v


class SteamControllerImu(object):
    def __init__(self):
        rospy.loginfo("Initializing imu publisher")
        self.imu_pub = rospy.Publisher('/imu', Imu, queue_size=5)
        rospy.loginfo("Publishing Imu at: " + self.imu_pub.resolved_name)
        self.ps_pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=5)
        # Tried to visualize with wrench the angular velocity but it didnt look good
        # self.wrench_pub = rospy.Publisher('/angular_vel', WrenchStamped, queue_size=5)

        try:
            sc = SteamController(callback=self.controller_cb)
            sc._sendControl(struct.pack('>' + 'I' * 6,
                                        0x87153284,
                                        0x03180000,
                                        0x31020008,
                                        0x07000707,
                                        0x00301400,
                                        0x2f010000))
            sc.run()
        except Exception as e:
            sys.stderr.write(str(e) + '\n')

    def controller_cb(self, sc, sci):
        print(sci)
        i = Imu()
        i.header.stamp = rospy.Time.now()
        i.header.frame_id = 'steamcontroller'
        i.orientation = create_quaternion(sci.q1, sci.q2, sci.q3, sci.q4)
        i.angular_velocity = create_vector3(sci.groll, sci.gpitch, sci.gyaw)
        self.imu_pub.publish(i)

        ps = PoseStamped()
        ps.header = i.header
        ps.pose.orientation = i.orientation
        self.ps_pub.publish(ps)

        # ws = WrenchStamped()
        # ws.header = ps.header
        # ws.wrench.torque = i.angular_velocity
        # self.wrench_pub.publish(ws)


if __name__ == '__main__':
    rospy.init_node('steam_controller_imu')
    sci = SteamControllerImu()