|
| 1 | +#!/usr/bin/env python2 |
| 2 | + |
| 3 | +import errno |
| 4 | +import math |
| 5 | +import socket |
| 6 | +import sys |
| 7 | +import threading |
| 8 | +import time |
| 9 | + |
| 10 | +import rospy |
| 11 | +import zmq |
| 12 | +import msgpack |
| 13 | + |
| 14 | +from sensor_msgs.msg import Imu |
| 15 | + |
| 16 | + |
| 17 | +def py3round(f): |
| 18 | + if abs(round(f) - f) == 0.5: |
| 19 | + return int(2.0 * round(f / 2.0)) |
| 20 | + return int(round(f)) |
| 21 | + |
| 22 | + |
| 23 | +def wait_for_master(): |
| 24 | + """Return when /use_sim_time is set in rosparams. If rospy.init_node is called before /use_sim_time |
| 25 | + is set, it uses real time.""" |
| 26 | + print("Waiting for rosmaster") |
| 27 | + start = time.time() |
| 28 | + last_params = [] |
| 29 | + while not rospy.is_shutdown(): |
| 30 | + try: |
| 31 | + params = rospy.get_param_names() |
| 32 | + if params != last_params: |
| 33 | + print(time.time()-start, params) |
| 34 | + if '/use_sim_time' in params: |
| 35 | + return True |
| 36 | + last_params = params |
| 37 | + time.sleep(0.01) |
| 38 | + except socket.error as serr: |
| 39 | + if serr.errno != errno.ECONNREFUSED: |
| 40 | + raise serr # re-raise error if its not the one we want |
| 41 | + time.sleep(0.5) |
| 42 | + |
| 43 | +class Bus: |
| 44 | + def __init__(self): |
| 45 | + self.lock = threading.Lock() |
| 46 | + context = zmq.Context.instance() |
| 47 | + self.push = context.socket(zmq.PUSH) |
| 48 | + self.push.setsockopt(zmq.LINGER, 100) # milliseconds |
| 49 | + self.push.bind('tcp://*:5565') |
| 50 | + |
| 51 | + def register(self, *outputs): |
| 52 | + pass |
| 53 | + |
| 54 | + def publish(self, channel, data): |
| 55 | + raw = msgpack.packb(data, use_bin_type=True) |
| 56 | + with self.lock: |
| 57 | + self.push.send_multipart([channel, raw]) |
| 58 | + |
| 59 | + |
| 60 | +class main: |
| 61 | + def __init__(self): |
| 62 | + # get cloudsim ready |
| 63 | + robot_name = sys.argv[1] |
| 64 | + imu_name = '/'+robot_name+'/imu/data' |
| 65 | + wait_for_master() |
| 66 | + rospy.init_node('imu2osgar', log_level=rospy.DEBUG) |
| 67 | + rospy.loginfo("waiting for {}".format(imu_name)) |
| 68 | + rospy.wait_for_message(imu_name, Imu) |
| 69 | + rospy.sleep(2) |
| 70 | + self.imu_count = 0 |
| 71 | + |
| 72 | + # start |
| 73 | + self.bus = Bus() |
| 74 | + self.bus.register('rot', 'acc', 'orientation') |
| 75 | + rospy.Subscriber(imu_name, Imu, self.imu) |
| 76 | + rospy.spin() |
| 77 | + |
| 78 | + def imu(self, msg): |
| 79 | + self.imu_count += 1 |
| 80 | + rospy.loginfo_throttle(10, "imu callback: {}".format(self.imu_count)) |
| 81 | + acc = [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z] |
| 82 | + orientation = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w] |
| 83 | + |
| 84 | + # copy & paste from rosmsg |
| 85 | + q0, q1, q2, q3 = orientation # quaternion |
| 86 | + x = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) |
| 87 | + y = math.asin(2 * (q0 * q2 - q3 * q1)) |
| 88 | + z = math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) |
| 89 | + rot = [x, y, z] |
| 90 | + # end of copy & paste from rosmsg |
| 91 | + |
| 92 | + self.bus.publish('rot', [py3round(math.degrees(angle) * 100) for angle in rot]) |
| 93 | + self.bus.publish('acc', [py3round(x * 1000) for x in acc]) |
| 94 | + self.bus.publish('orientation', orientation) |
| 95 | + # preliminary suggestion for combined message |
| 96 | + #angular_velocity = [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z] |
| 97 | + #data = [ |
| 98 | + # msg.header.stamp.to_nsec()/1000000, # time in milliseconds |
| 99 | + # orientation, |
| 100 | + # angular_velocity, |
| 101 | + # acc, |
| 102 | + #] |
| 103 | + #self.bus.publish('imu', data) |
| 104 | + |
| 105 | + |
| 106 | +if __name__ == '__main__': |
| 107 | + if len(sys.argv) < 2: |
| 108 | + print("need robot name as argument") |
| 109 | + raise SystemExit(1) |
| 110 | + main() |
| 111 | + |
0 commit comments