Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Publishing the imu of the steam controller
#!/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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.