Python sensor_msgs.msg.Imu() Examples

def main():
  if len(sys.argv) < 2:
    print("Usage: {} dataset_name".format(sys.argv[0]))
    exit(1)

  file_name = sys.argv[1]
  log_file = h5py.File('../dataset/log/{}.h5'.format(file_name))
  camera_file = h5py.File('../dataset/camera/{}.h5'.format(file_name))  

  zipped_log = izip(
    log_file['times'],
    log_file['fiber_accel'],
    log_file['fiber_gyro'])

  with rosbag.Bag('{}.bag'.format(file_name), 'w') as bag:
    bar = Bar('Camera', max=len(camera_file['X']))
    for i, img_data in enumerate(camera_file['X']):
      m_img = Image()
      m_img.header.stamp = rospy.Time.from_sec(0.01 * i)
      m_img.height = img_data.shape[1]
      m_img.width = img_data.shape[2]
      m_img.step = 3 * img_data.shape[2]
      m_img.encoding = 'rgb8'
      m_img.data = np.transpose(img_data, (1, 2, 0)).flatten().tolist()
      
      bag.write('/camera/image_raw', m_img, m_img.header.stamp)
      bar.next()
      
    bar.finish()

    bar = Bar('IMU', max=len(log_file['times']))
    for time, v_accel, v_gyro in zipped_log:
      m_imu = Imu()
      m_imu.header.stamp = rospy.Time.from_sec(time)
      [setattr(m_imu.linear_acceleration, c, v_accel[i]) for i, c in enumerate('xyz')]
      [setattr(m_imu.angular_velocity, c, v_gyro[i]) for i, c in enumerate('xyz')]

      bag.write('/fiber_imu', m_imu, m_imu.header.stamp)
      bar.next()

    bar.finish() 

def save_imu_data(bag, kitti, imu_frame_id, topic):
    print("Exporting IMU")
    for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
        q = tf.transformations.quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw)
        imu = Imu()
        imu.header.frame_id = imu_frame_id
        imu.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        imu.orientation.x = q[0]
        imu.orientation.y = q[1]
        imu.orientation.z = q[2]
        imu.orientation.w = q[3]
        imu.linear_acceleration.x = oxts.packet.af
        imu.linear_acceleration.y = oxts.packet.al
        imu.linear_acceleration.z = oxts.packet.au
        imu.angular_velocity.x = oxts.packet.wf
        imu.angular_velocity.y = oxts.packet.wl
        imu.angular_velocity.z = oxts.packet.wu
        bag.write(topic, imu, t=imu.header.stamp) 

def save_imu_data(bag, kitti, imu_frame_id, topic):
    print("Exporting IMU")
    for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
        q = tf.transformations.quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw)
        imu = Imu()
        imu.header.frame_id = imu_frame_id
        imu.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
        imu.orientation.x = q[0]
        imu.orientation.y = q[1]
        imu.orientation.z = q[2]
        imu.orientation.w = q[3]
        imu.linear_acceleration.x = oxts.packet.af
        imu.linear_acceleration.y = oxts.packet.al
        imu.linear_acceleration.z = oxts.packet.au
        imu.angular_velocity.x = oxts.packet.wf
        imu.angular_velocity.y = oxts.packet.wl
        imu.angular_velocity.z = oxts.packet.wu
        bag.write(topic, imu, t=imu.header.stamp) 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      self.last_imu_angle = self.imu_angle
      rospy.sleep(10)
      rospy.loginfo("imu_drift:"+str((self.imu_angle-self.last_imu_angle)*180/3.1415926)) 

def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.3)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 

def __init__(self):
        self.throttleInitialized = False
        self.joy_timeout = 2.0
        self.lid_timeout = 0.30
        self.cnn_timeout = 0.1
        self.joy_time = time.time()
        self.lid_time = time.time()
        self.cnn_time = time.time()
        self.controller = XBox360()
        self.joy_cmd = TwistStamped()
        self.lid_cmd = TwistStamped()
        self.cnn_cmd = TwistStamped()
        self.cruiseControl = False
        self.cruiseThrottle = 0.5
        self.steeringAngle = 0.5
        self.throttle = 0.5
        self.trim = 0.0
        ##self.throttleLock = Lock()
        print "cmd_control"
       
        rospy.Subscriber("/imu", Imu, self.imuCB, queue_size=5)
        rospy.Subscriber("/lidar_twist", TwistStamped, self.lidarTwistCB, queue_size=5)
        rospy.Subscriber("/neural_twist", TwistStamped, self.neuralTwistCB, queue_size=5)
        rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=5)
        self.vel_pub = rospy.Publisher("/cmd_vel", TwistStamped, queue_size = 1) 
        #self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=1)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)        
        rospy.init_node ('cmd_control',anonymous=True)
        rate = rospy.Rate(66)
        while not rospy.is_shutdown():
            self.cmdRouter()
            rate.sleep() 

def init():
    global q_imu
    global fake_measure, fake_distance

    q_imu = Imu().orientation

    fake_distance = [3.0, 6.0, 0.0, 0.0, 0.0, 0.0]
    fake_measure = [3.0, 6.0, 0.0, 0.0, 0.0, 0.0]

    rospy.init_node('quaternion_test')
    imu_sub         = rospy.Subscriber('mavros/imu/data', Imu, imu_callback) 

def sensor_data_updated(self, carla_imu_measurement):
        """
        Function to transform a received imu measurement into a ROS Imu message

        :param carla_imu_measurement: carla imu measurement object
        :type carla_imu_measurement: carla.IMUMeasurement
        """
        imu_msg = Imu()
        imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp)

        # Carla uses a left-handed coordinate convention (X forward, Y right, Z up).
        # Here, these measurements are converted to the right-handed ROS convention
        #  (X forward, Y left, Z up).
        imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x
        imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y
        imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z

        imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x
        imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y
        imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z

        imu_rotation = carla_imu_measurement.transform.rotation

        quat = trans.carla_rotation_to_numpy_quaternion(imu_rotation)
        imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat)
        self.publish_message(
            self.get_topic_prefix(), imu_msg) 

def test_imu(self):
        """
        Tests IMU sensor node
        """
        rospy.init_node('test_node', anonymous=True)
        print("testing  for ros bridge")
        msg = rospy.wait_for_message("/carla/ego_vehicle/imu", Imu, timeout=15)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/imu")
        self.assertNotEqual(msg.linear_acceleration, 0.0)
        self.assertNotEqual(msg.angular_velocity, 0.0)
        self.assertNotEqual(msg.orientation, 0.0) 

def __init__(self):
        self._MsgData = Imu()
        self._MsgPub = rospy.Publisher('/movo/feedback/sic_imu', Imu, queue_size=10)
        self._MsgData.header.frame_id = 'sic_imu_frame'
        self._seq = 0 

def handle_received_line(self, line):
        """Calculate orientation from accelerometer and gyrometer"""
        self._counter = self._counter + 1
        self._SerialPublisher.publish(String(str(self._counter) + ", in:  " + line))

        if line:
            lineParts = line.split('\t')
            try:
                if lineParts[0] == 'b':
                    self._battery_value = float(lineParts[1])
                    self._battery_pub.publish(self._battery_value)

                if lineParts[0] == 'i':
#                    self._qx, self._qy, self._qz, self._qw, \
#                    self._gx, self._gy, self._gz, \
#                    self._ax, self._ay, self.az = [float(i) for i in lineParts[1:11]]
                    self._qx = float(lineParts[1])
                    self._qy = float(lineParts[2])
                    self._qz = float(lineParts[3])
                    self._qw = float(lineParts[4])
                    self._gx = float(lineParts[5])
                    self._gy = float(lineParts[6])
                    self._gz = float(lineParts[7])
                    self._ax = float(lineParts[8])
                    self._ay = float(lineParts[9])
                    self._az = float(lineParts[10])

                    imu_msg = Imu()
                    header = Header()
                    header.stamp = rospy.Time.now()
                    header.frame_id = self._frame_id
                    imu_msg.header = header

                    imu_msg.orientation_covariance = self._imu_data.orientation_covariance
                    imu_msg.angular_velocity_covariance = self._imu_data.angular_velocity_covariance
                    imu_msg.linear_acceleration_covariance = self._imu_data.linear_acceleration_covariance

                    imu_msg.orientation.x = self._qx
                    imu_msg.orientation.y = self._qy
                    imu_msg.orientation.z = self._qz
                    imu_msg.orientation.w = self._qw
                    imu_msg.angular_velocity.x = self._gx
                    imu_msg.angular_velocity.y = self._gy
                    imu_msg.angular_velocity.z = self._gz
                    imu_msg.linear_acceleration.x = self._ax
                    imu_msg.linear_acceleration.y = self._ay
                    imu_msg.linear_acceleration.z = self._az
                    # q_rot = quaternion_from_euler(self.pi, -self.pi/2, 0)
                    # q_ori = Quaternion(self._qx, self._qy, self._qz, self._qw)
                    # imu_msg.orientation = quaternion_multiply(q_ori, q_rot)
                    self._imu_pub.publish(imu_msg)
            except:
                rospy.logwarn("Error in Sensor values")
                rospy.logwarn(lineParts) 

def pub_loop():
	rospy.init_node('state_publisher', anonymous=True)	
	rospy.Subscriber('/gps/fix', NavSatFix, parse_gps_fix, queue_size=1)
	rospy.Subscriber('/gps/vel', TwistWithCovarianceStamped, parse_gps_vel, queue_size=1)
	rospy.Subscriber('/imu/data', Imu, parse_imu_data, queue_size=1)
	rospy.Subscriber('/vehicle/steering', SteeringReport, parse_steering_angle, queue_size=1)

	if not (rospy.has_param('lat0') and rospy.has_param('lon0') and rospy.has_param('yaw0')):
		raise ValueError('Invalid rosparam global origin provided!')

	if not rospy.has_param('time_check_on'):
		raise ValueError('Did not specify if time validity should be checked!')

	LAT0 = rospy.get_param('lat0')
	LON0 = rospy.get_param('lon0')
	YAW0 = rospy.get_param('yaw0')
	time_check_on = rospy.get_param('time_check_on')
	
	state_pub = rospy.Publisher('state_est', state_est, queue_size=1)

	r = rospy.Rate(100)
	while not rospy.is_shutdown():		
		
		if None in (lat, lon, psi, vel, acc_filt, df): 
			r.sleep() # If the vehicle state info has not been received.
			continue

		curr_state = state_est()
		curr_state.header.stamp = rospy.Time.now()
		
		# TODO: time validity check, only publish if data is fresh
		#if time_check_on and not time_valid(curr_state.header.stamp,[tm_vel, tm_df, tm_imu, tm_gps]):
		#	r.sleep()
		#	continue

		curr_state.lat = lat
		curr_state.lon = lon

		X,Y = latlon_to_XY(LAT0, LON0, lat, lon)

		curr_state.x   = X
		curr_state.y   = Y
		curr_state.psi = psi
		curr_state.v   = vel
		
		curr_state.a   = acc_filt
		curr_state.df  = df

		state_pub.publish(curr_state)
		r.sleep()