def main():
rospy.init_node("imu")
port = rospy.get_param("~port", "GPG3_AD1")
sensor = InertialMeasurementUnit(bus=port)
pub_imu = rospy.Publisher("~imu", Imu, queue_size=10)
pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10)
pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10)
br = TransformBroadcaster()
msg_imu = Imu()
msg_temp = Temperature()
msg_magn = MagneticField()
hdr = Header(stamp=rospy.Time.now(), frame_id="IMU")
rate = rospy.Rate(rospy.get_param('~hz', 30))
while not rospy.is_shutdown():
q = sensor.read_quaternion() # x,y,z,w
mag = sensor.read_magnetometer() # micro Tesla (µT)
gyro = sensor.read_gyroscope() # deg/second
accel = sensor.read_accelerometer() # m/s²
temp = sensor.read_temperature() # °C
msg_imu.header = hdr
hdr.stamp = rospy.Time.now()
msg_temp.header = hdr
msg_temp.temperature = temp
pub_temp.publish(msg_temp)
msg_imu.header = hdr
msg_imu.linear_acceleration.x = accel[0]
msg_imu.linear_acceleration.y = accel[1]
msg_imu.linear_acceleration.z = accel[2]
msg_imu.angular_velocity.x = math.radians(gyro[0])
msg_imu.angular_velocity.y = math.radians(gyro[1])
msg_imu.angular_velocity.z = math.radians(gyro[2])
msg_imu.orientation.w = q[3]
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
pub_imu.publish(msg_imu)
msg_magn.header = hdr
msg_magn.magnetic_field.x = mag[0] * 1e-6
msg_magn.magnetic_field.y = mag[1] * 1e-6
msg_magn.magnetic_field.z = mag[2] * 1e-6
pub_magn.publish(msg_magn)
transform = TransformStamped(header=Header(stamp=rospy.Time.now(),
frame_id="world"),
child_frame_id="IMU")
transform.transform.rotation = msg_imu.orientation
br.sendTransformMessage(transform)
rate.sleep()
def sensor_interface():
rospy.init_node('imu_razor9dof', anonymous = False)
interface.flushInput()
# Perform a loop checking on the enabled status of the drivers
while not rospy.is_shutdown():
# Fetch a JSON message from the controller and process (or atleast attempt to)
try:
# Get the object and create some messages
_object = json.loads(interface.readline())
_imu = Imu();
_heading = Float32()
# Get the imu data (f**k quaternions!!)
_imu.header = rospy.Header()
roll = _object['angles'][0]
pitch = _object['angles'][1]
yaw = _object['angles'][2]
#_imu.orientation = tf::createQuaternionFromRPY(roll, pitch, yaw)
_imu.linear_acceleration.x = _object['accel'][0]
_imu.linear_acceleration.y = _object['accel'][1]
_imu.linear_acceleration.z = _object['accel'][2]
# Get the heading data
#_heading.data = _object['heading']
_heading.data = _object['angles'][2]
# Publish the data
imuPublisher.publish(_imu);
cmpPublisher.publish(_heading);
except json.decoder.JSONDecodeError:
rospy.logwarn("Invalid message received")
except: pass
def imuCallback(self, imu):
self.acceleration[0] = imu.linear_acceleration.x
self.acceleration[1] = imu.linear_acceleration.y
self.acceleration[2] = imu.linear_acceleration.z
#print("Accel", self.acceleration)
accel_temp = np.array([-imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z])
#print("AccelRaw",accel_temp)
accel_temp_trans = self.getQuaternionOrientation().rotate(accel_temp)
accel_temp_trans[0]= -accel_temp_trans[0]
accel_temp_trans[1]= -accel_temp_trans[1]
accel_temp_trans[2] = -(accel_temp_trans[2] - 9.81)
self.acceleration_rot = accel_temp_trans
self.accelMedianX = np.roll(self.accelMedianX, 1)
self.accelMedianY = np.roll(self.accelMedianY, 1)
self.accelMedianZ = np.roll(self.accelMedianZ, 1)
self.accelMedianX[0] = accel_temp_trans[0]
self.accelMedianY[1] = accel_temp_trans[1]
self.accelMedianZ[2] = accel_temp_trans[2]
self.filteredAccelValue[0]= (np.sum(self.accelMedianX) / len(self.accelMedianX))
self.filteredAccelValue[1] = (np.sum(self.accelMedianY) / len(self.accelMedianY))
self.filteredAccelValue[2] = (np.sum(self.accelMedianZ) / len(self.accelMedianZ))
# print(imu.linear_acceleration.x)
#print("X",self.accelMedianX)
#print("X Filtered", self.filteredAccelValue[0])
accelNED=Imu()
accelNED.header=imu.header
accelNED.linear_acceleration.x=self.filteredAccelValue[0]
accelNED.linear_acceleration.y = self.filteredAccelValue[1]
accelNED.linear_acceleration.z = self.filteredAccelValue[2]
self.accel_publish.publish(accelNED)
def convert_imu(imu):
ret = Imu()
ret.header = convert_utime(imu.utime)
ret.orientation = rpy_to_quat(*imu.tb_angles)
ret.orientation_covariance = [0]*9
gyro = imu.gyro
w = ret.angular_velocity
w.x, w.y, w.z = gyro
# 5 dps tolerance at 0 with 3 percent linear tolerance
# 2 percent cross axis sensitivity
rps = 5/180*math.pi
cross = 0.02**2
ret.angular_velocity_covariance = [
(rps + gyro[0]*.03)**2, cross*(gyro[0]*gyro[1]), cross*(gyro[0]*gyro[2]),
cross*(gyro[0]*gyro[1]), (rps + gyro[1]*.03)**2, cross*(gyro[1]*gyro[2]),
cross*(gyro[0]*gyro[2]), cross*(gyro[1]*gyro[2]), (rps + gyro[2]*.03)**2
]
accel = imu.accel
a = ret.linear_acceleration
a.x, a.y, a.z = accel
# 3 percent sensitivity with 2 percent cross axis sensitivity
ret.linear_acceleration_covariance = [
(accel[0]*0.03)**2, cross*(accel[0]*accel[1]), cross*(accel[0]*accel[2]),
cross*(accel[0]*accel[1]), (accel[1]*0.03)**2, cross*(accel[1]*accel[2]),
cross*(accel[0]*accel[2]), cross*(accel[1]*accel[2]), (accel[2]*0.03)**2
]
return ret
def Publish(self):
imu_data = self._hal_proxy.GetImu()
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self._frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.orientation.w = imu_data['orientation']['w']
imu_msg.orientation.x = imu_data['orientation']['x']
imu_msg.orientation.y = imu_data['orientation']['y']
imu_msg.orientation.z = imu_data['orientation']['z']
imu_msg.linear_acceleration.x = imu_data['linear_accel']['x']
imu_msg.linear_acceleration.y = imu_data['linear_accel']['y']
imu_msg.linear_acceleration.z = imu_data['linear_accel']['z']
imu_msg.angular_velocity.x = imu_data['angular_velocity']['x']
imu_msg.angular_velocity.y = imu_data['angular_velocity']['y']
imu_msg.angular_velocity.z = imu_data['angular_velocity']['z']
# Read the x, y, z and heading
self._publisher.publish(imu_msg)
def run(self):
while not rospy.is_shutdown():
time_now = time.time()
if time_now - self.timestamp > 0.5:
self.cmd = (0, 0)
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
self.heartBeat.publish("I am alive from Glew")
if self.msg != None:
self.publisher.publish(self.msg.linear.y)
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0, 0)
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 imuResponseCallback(self, msg):
#############################################################################
self.num_imu_packets += 1
if self.num_imu_packets > 100:
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = "imu_link"
def do_transform_imu(imu_in, transform):
transform = copy.deepcopy(transform)
kdl_tf = transform_to_kdl(transform)
quaternion = (kdl_tf.M * PyKDL.Rotation.Quaternion(
imu_in.orientation.x,
imu_in.orientation.y,
imu_in.orientation.z,
imu_in.orientation.w)).GetQuaternion()
angular_velocity = kdl_tf * PyKDL.Vector(
imu_in.angular_velocity.x,
imu_in.angular_velocity.y,
imu_in.angular_velocity.z)
linear_acceleration = kdl_tf * PyKDL.Vector(
imu_in.linear_acceleration.x,
imu_in.linear_acceleration.y,
imu_in.linear_acceleration.z)
imu_out = Imu()
imu_out.header = transform.header
imu_out.orientation.x = quaternion[0]
imu_out.orientation.y = quaternion[1]
imu_out.orientation.z = quaternion[2]
imu_out.orientation.w = quaternion[3]
imu_out.angular_velocity.x = angular_velocity[0]
imu_out.angular_velocity.y = angular_velocity[1]
imu_out.angular_velocity.z = angular_velocity[2]
imu_out.linear_acceleration.x = linear_acceleration[0]
imu_out.linear_acceleration.y = linear_acceleration[1]
imu_out.linear_acceleration.z = linear_acceleration[2]
return imu_out
def imuResponseCallback(self, msg):
#############################################################################
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.orientation = msg.orientation
imu_msg.angular_velocity = msg.angular_velocity
imu_msg.linear_acceleration = msg.linear_acceleration
imu_msg.orientation_covariance[0] = msg.orientation_covariance[0]
imu_msg.orientation_covariance[4] = msg.orientation_covariance[4]
if self.rtkEnabled == 0:
imu_msg.orientation_covariance[8] = msg.orientation_covariance[8]
else:
imu_msg.orientation_covariance[8] = msg.orientation_covariance[
8] + 99999 / (1 + math.exp(-8 * (abs(self.totalVel) - 2))
) # sigmoid function to mix yaw from the IMU
imu_msg.angular_velocity_covariance[
0] = msg.angular_velocity_covariance[0]
imu_msg.angular_velocity_covariance[
4] = msg.angular_velocity_covariance[4]
imu_msg.angular_velocity_covariance[
8] = msg.angular_velocity_covariance[8]
imu_msg.linear_acceleration_covariance[
0] = msg.linear_acceleration_covariance[0]
imu_msg.linear_acceleration_covariance[
4] = msg.linear_acceleration_covariance[4]
imu_msg.linear_acceleration_covariance[
8] = msg.linear_acceleration_covariance[8]
#rospy.logerr("Vel: %f Z Rot Covariance: %f", self.totalVel, imu_msg.orientation_covariance[8])
self.imuPub.publish(imu_msg)
def talker():
imu_data = Imu()
odom = Odometry()
imu_pub = rospy.Publisher('Imu', Imu, queue_size=1)
odom_pub = rospy.Publisher('Odom', Odometry, queue_size=1)
rospy.init_node('LISTEN_ODOM_IMU')
rate = rospy.Rate(100)
while not rospy.is_shutdown():
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = 'IMU (BASE)'
imu_data.header = h
q, g, a, twist_linear, twist_angular, pose_pt = serial_listen()
imu_data.orientation = q
imu_data.angular_velocity = g
imu_data.linear_acceleration = a
covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
imu_data.orientation_covariance = covariance
imu_data.angular_velocity_covariance = covariance
imu_data.linear_acceleration_covariance = covariance
odom.twist.twist.linear = twist_linear
odom.twist.twist.angular = twist_angular
odom.pose.pose.position = pose_pt
imu_pub.publish(imu_data)
odom_pub.publish(odom)
rospy.loginfo('PUBLISHED...')
rate.sleep()
def publishIMU(publisher):
imu_raw = Imu()
# Header:
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'imu_filter'
imu_raw.header = h
# Data:
imu_raw.orientation_covariance[0] = -1
imu_raw.linear_acceleration.x = 0
imu_raw.linear_acceleration.y = 0
imu_raw.linear_acceleration.z = 0
imu_raw.linear_acceleration_covariance[0] = -1
imu_raw.angular_velocity.x = 0
imu_raw.angular_velocity.y = 0
imu_raw.angular_velocity.z = 0
imu_raw.angular_velocity_covariance[0] = -1
imu_raw.orientation.w = 1
imu_raw.orientation.x = 0
imu_raw.orientation.y = 0
imu_raw.orientation.z = 0
imu_raw.orientation_covariance[0] = -1
publisher.publish(imu_raw)
def on_imu(channel, data):
m = ins_t.decode(data)
pyTime = rospy.Time.from_sec(m.utime * 1E-6)
#print "Received ins message", pyTime
pose = PoseStamped()
t = rospy.get_rostime()
pose.header.stamp = pyTime
pose.header.frame_id = 'imu_link'
pose.pose.orientation.w = m.quat[0]
pose.pose.orientation.x = m.quat[1]
pose.pose.orientation.y = m.quat[2]
pose.pose.orientation.z = m.quat[3]
imuRosPosePub.publish(pose)
imu = Imu()
imu.header = pose.header
imu.orientation = pose.pose.orientation
imu.angular_velocity.x = m.gyro[0]
imu.angular_velocity.y = m.gyro[1]
imu.angular_velocity.z = m.gyro[2]
imu.linear_acceleration.x = m.accel[0]
imu.linear_acceleration.y = m.accel[1]
imu.linear_acceleration.z = m.accel[2]
imuRosPub.publish(imu)
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
roll, pitch, yaw = trans.carla_rotation_to_RPY(
carla_imu_measurement.transform.rotation)
quat = euler2quat(roll, pitch, yaw)
imu_msg.orientation.w = quat[0]
imu_msg.orientation.x = quat[1]
imu_msg.orientation.y = quat[2]
imu_msg.orientation.z = quat[3]
self.imu_publisher.publish(imu_msg)
def decode_regs_encode_msg(data, header=None, calibration=False):
global td
ax = np.int16(int(binascii.hexlify(data[0:2]), 16)) * IMUPI_A_GAIN * M_G
ay = np.int16(int(binascii.hexlify(data[2:4]), 16)) * IMUPI_A_GAIN * M_G
az = np.int16(int(binascii.hexlify(data[4:6]), 16)) * IMUPI_A_GAIN * M_G
gx = np.int16(int(binascii.hexlify(data[8:10]),
16)) * IMUPI_G_GAIN * M_PI / 180 - GYRO_OFF_X
gy = np.int16(int(binascii.hexlify(data[10:12]),
16)) * IMUPI_G_GAIN * M_PI / 180 - GYRO_OFF_Y
gz = np.int16(int(binascii.hexlify(data[12:14]),
16)) * IMUPI_G_GAIN * M_PI / 180 - GYRO_OFF_Z
if calibration == False:
msg = Imu()
msg.linear_acceleration.x = ax
msg.linear_acceleration.y = ay
msg.linear_acceleration.z = az
msg.angular_velocity.x = gx
msg.angular_velocity.y = gy
msg.angular_velocity.z = gz
msg.header = header
msg.header.stamp = msg.header.stamp + td
return msg
else:
#print('ax: {0}, ay: {1}, az: {2}, gx: {3}, gy: {4}, gz: {5}'.format(ax, ay, az, gx, gy, gz))
return ax, ay, az, gx, gy, gz
def imuCallback(self, data):
self.stamp = data.header.stamp
dt = self.stamp - self.last_stamp
self.last_stamp = data.header.stamp
# print("dt = ", dt.to_sec())
data_output = Imu()
data_output.orientation.w = self.ori_w.predic(data.orientation.w,
dt.to_sec())
data_output.orientation.x = self.ori_x.predic(data.orientation.x if False else 0.0,
dt.to_sec())
data_output.orientation.y = self.ori_y.predic(data.orientation.y if False else 0.0,
dt.to_sec())
data_output.orientation.z = self.ori_z.predic(data.orientation.z,
dt.to_sec())
data_output.angular_velocity.x = self.av_x.predic(
data.angular_velocity.x if False else 0.0, dt.to_sec())
data_output.angular_velocity.y = self.av_y.predic(
data.angular_velocity.y if False else 0.0, dt.to_sec())
data_output.angular_velocity.z = self.av_z.predic(
data.angular_velocity.z, dt.to_sec())
data_output.linear_acceleration.x = self.la_x.predic(
data.linear_acceleration.x, dt.to_sec())
data_output.linear_acceleration.y = self.la_y.predic(
data.linear_acceleration.y, dt.to_sec())
data_output.linear_acceleration.z = self.la_z.predic(
data.linear_acceleration.z if False else 0.0, dt.to_sec())
data_output.header = data.header
self.pub_imuCorrect.publish(data_output)
def unpackIMU(self, data): imu_msg = Imu() #Unpack Header imu_msg.header = self.unpackIMUHeader(data[0:19]) #Unpack Orientation Message quat = self.bytestr_to_array(data[19:19 + (4*8)]) imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] #Unpack Orientation Covariance imu_msg.orientation_covariance = list(self.bytestr_to_array(data[55:(55 + (9*8))])) #Unpack Angular Velocity ang = self.bytestr_to_array(data[127: 127 + (3*8)]) imu_msg.angular_velocity.x = ang[0] imu_msg.angular_velocity.y = ang[1] imu_msg.angular_velocity.z = ang[2] #Unpack Angular Velocity Covariance imu_msg.angular_velocity_covariance = list(self.bytestr_to_array(data[155:(155 + (9*8))])) #Unpack Linear Acceleration lin = self.bytestr_to_array(data[227: 227 + (3*8)]) imu_msg.linear_acceleration.x = lin[0] imu_msg.linear_acceleration.y = lin[1] imu_msg.linear_acceleration.z = lin[2] #Unpack Linear Acceleration Covariance imu_msg.linear_acceleration_covariance = list(self.bytestr_to_array(data[255:(255 + (9*8))])) return imu_msg
def publish(self, theta, omega):
msg = Imu()
msg.header = self.header
msg.angular_velocity.x = omega[0]*1
msg.angular_velocity.y = omega[1]*1
msg.angular_velocity.z = omega[2]*1
msg.orientation = self.thetaToOrientation(theta)
grav = np.array([0,0,self.gravity], dtype=np.float64)
r = self.thetaToRotation(theta)
grav = r.apply(grav, True)
pureAcceleration = self.accel - grav
msg.linear_acceleration.x = pureAcceleration[0]*1
msg.linear_acceleration.y = pureAcceleration[1]*1
msg.linear_acceleration.z = pureAcceleration[2]*1
cov = np.zeros(9, dtype=np.float64)
msg.angular_velocity_covariance = cov
msg.orientation_covariance = cov
msg.linear_acceleration_covariance = cov
self.publisher.publish(msg)
def odom_subscriber_cb(data):
imu_msg = Imu()
imu_msg.header = data.header
imu_msg.header.frame_id = 'base_link'
imu_msg.orientation = data.pose.pose.orientation
imu_msg.linear_acceleration.z = -10
pub.publish(imu_msg)
def publish_imu(self):
if not self.last_euler or not self.last_accelerometer:
return
with self._imu_pub_lock:
imu_msg = Imu()
imu_msg.header = Header()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = c.BASE_LINK
# Our sensor returns Euler angles in degrees, but ROS requires radians.
roll = self.last_euler['x'] * pi / 180.
pitch = self.last_euler['y'] * pi / 180.
yaw = self.last_euler['z'] * pi / 180.
# http://answers.ros.org/question/69754/quaternion-transformations-in-python/
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
imu_msg.orientation.x = quaternion[0]
imu_msg.orientation.y = quaternion[1]
imu_msg.orientation.z = quaternion[2]
imu_msg.orientation.w = quaternion[3]
#imu_msg.angular_velocity_covariance = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
# imu_msg.angular_velocity.x = ns.omgx();
# imu_msg.angular_velocity.y = ns.omgy();
# imu_msg.angular_velocity.z = ns.omgz();
#imu_msg.angular_velocity_covariance = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
imu_msg.linear_acceleration.x = self.last_accelerometer['x']
imu_msg.linear_acceleration.y = self.last_accelerometer['y']
imu_msg.linear_acceleration.z = self.last_accelerometer['z']
#imu_msg.angular_velocity_covariance = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
self.imu_pub.publish(imu_msg)
def talker():
pub = rospy.Publisher('imu', Imu, queue_size=10)
rospy.init_node('imusensor', anonymous=True)
r = rospy.Rate(500)
while not rospy.is_shutdown():
#str = "hello world %s"%print_gyro().__str__()
arr = print_gyro()
i = Imu()
i.header = std_msgs.msg.Header()
i.header.frame_id="my_frame"
i.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
i.linear_acceleration.x=arr[0][0]
i.linear_acceleration.y=arr[0][1]
i.linear_acceleration.z=arr[0][2]
i.angular_velocity.x=arr[1][0]
i.angular_velocity.y=arr[1][1]
i.angular_velocity.z=arr[1][2]
i.orientation.x=0
i.orientation.y=0
i.orientation.z=0
i.orientation.w=0
#rospy.loginfo(str)
pub.publish(i)
r.sleep()
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
enc_msg = Int32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
self.heartBeat.publish("I am alive")
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0,0)
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)
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 = tf.transformations.quaternion_from_euler(math.radians(imu_rotation.roll),
math.radians(imu_rotation.pitch),
math.radians(imu_rotation.yaw))
imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat)
self.publish_message(
self.get_topic_prefix(), imu_msg)
def imu_msg_callback(msg, cb_args):
pub = cb_args[0]
global last_lin_acc
global last_ang_vel
send = Imu()
send.header = msg.header
send.header.stamp = rospy.Time.now()
# send.header.frame_id = "camera_imu_frame"
# if msg.linear_acceleration_covariance[0] != -1.0:
# last_lin_acc.x = msg.linear_acceleration.z
# last_lin_acc.y = msg.linear_acceleration.x * -1
# last_lin_acc.z = msg.linear_acceleration.y * -1
# elif msg.angular_velocity_covariance[0] != -1.0:
# last_ang_vel.x = msg.angular_velocity.z
# last_ang_vel.y = msg.angular_velocity.x * -1
# last_ang_vel.z = msg.angular_velocity.y * -1
if msg.linear_acceleration_covariance[0] != -1.0:
last_lin_acc = msg.linear_acceleration
elif msg.angular_velocity_covariance[0] != -1.0:
last_ang_vel = msg.angular_velocity
send.angular_velocity = last_ang_vel
send.linear_acceleration = last_lin_acc
send.orientation_covariance[0] = -1.0
pub.publish(send)
def parseImuData(pData, qx_pub, qy_pub, qz_pub, qw_pub, imu_pub):
parseData = pData.split('|')
if len(parseData) == 6:
qxVal = float(parseData[1])
qyVal = float(parseData[2])
qzVal = float(parseData[3])
qwVal = float(parseData[4])
qx_pub.publish(qxVal)
qy_pub.publish(qyVal)
qz_pub.publish(qzVal)
qw_pub.publish(qwVal)
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = "base_link"
imu_msg.header = h
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
imu_msg.orientation.x = qxVal
imu_msg.orientation.y = qyVal
imu_msg.orientation.z = qzVal
imu_msg.orientation.w = qwVal
imu_pub.publish(imu_msg)
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(),
self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
enc_msg = Int32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
self.heartBeat.publish("I am alive")
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0, 0)
def parse_imu(self, data):
'''
Given data from jaguar imu, parse and return a standard IMU message.
Return None when given bad data, or no complete message was found in data.
'''
# Use regular expressions to extract complete message
# message format: $val,val,val,val,val,val,val,val,val,val#\n
hit = re.search(r"\$-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*#", data)
if not hit:
# if there are no hits, return None
return None
else:
imu_data = hit.group(0)
try:
# Try to format the data
imu_data = imu_data[1:-1].split(",")
#Format (from drrobot docs): seq, accelx, accely, accelz, gyroY, gyroZ, gyroX, magnetonX, magnetonY, magnetonZ
seq = int(imu_data[0])
accelx = float(imu_data[1])
accely = float(imu_data[2])
accelz = float(imu_data[3])
gyroy = float(imu_data[4])
gyroz = float(imu_data[5])
gyrox = float(imu_data[6])
magnetonx = float(imu_data[7])
magnetony = float(imu_data[8])
magnetonz = float(imu_data[9])
rot_mat = [ [float(imu_data[10]), float(imu_data[11]), float(imu_data[12])],
[float(imu_data[13]), float(imu_data[14]), float(imu_data[15])],
[float(imu_data[16]), float(imu_data[17]), float(imu_data[18])] ]
except:
# bad data in match, pass
return None
else:
# data formatted fine, build message and publish
# if we didn't get a magnetometer update, set to current reading
if magnetonz == 0:
magnetonx = self.current_mag[0]
magnetony = self.current_mag[1]
magnetonz = self.current_mag[2]
# otherwise, update current magnetometer
else:
self.current_mag = [magnetonx, magnetony, magnetonz]
# Calculate quaternion from given rotation matrix
quat = rot_mat_to_quat(rot_mat);
# Build message
msg = Imu()
msg.header = Header(stamp = rospy.Time.now(), frame_id = "imu")
msg.linear_acceleration = Vector3(accelx, accely, accelz)
msg.angular_velocity = Vector3(gyrox, gyroy, gyroz)
if quat != None: msg.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
return msg
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'quat'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
if(lineParts[0] == 'ypr'):
self._ax = float(lineParts[1])
self._ay = float(lineParts[2])
self._az = float(lineParts[3])
if(lineParts[0] == 'areal'):
self._lx = float(lineParts[1])
self._ly = float(lineParts[2])
self._lz = float(lineParts[3])
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
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._ax
imu_msg.angular_velocity.y = self._ay
imu_msg.angular_velocity.z = self._az
imu_msg.linear_acceleration.x = self._lx
imu_msg.linear_acceleration.y = self._ly
imu_msg.linear_acceleration.z = self._lz
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(
String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split('\t')
try:
if (lineParts[0] == 'e'):
self._first_encoder_value = long(lineParts[1])
self._second_encoder_value = long(lineParts[2])
self._third_encoder_value = long(lineParts[3])
#######################################################################################################################
self._First_Encoder.publish(self._first_encoder_value)
self._Second_Encoder.publish(self._second_encoder_value)
self._Third_Encoder.publish(self._third_encoder_value)
if (lineParts[0] == 'i'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
#######################################################################################################################
self._qx_.publish(self._qx)
self._qy_.publish(self._qy)
self._qz_.publish(self._qz)
self._qw_.publish(self._qw)
#######################################################################################################################
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def callback(data):
quaternion = tf.transformations.quaternion_from_euler(
data.angle.x, -data.angle.y, -data.angle.z + np.pi / 2)
imu_data = Imu()
imu_data.orientation.x = quaternion[0]
imu_data.orientation.y = quaternion[1]
imu_data.orientation.z = quaternion[2]
imu_data.orientation.w = quaternion[3]
imu_data.header = data.header
pub.publish(imu_data)
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 handleIMU(data):
rospy.logdebug("TorsoIMU received for conversion: %f %f", data.angleX, data.angleY)
imu_msg = Imu()
q = tf.quaternion_from_euler(data.angleX, data.angleY, 0.0)
imu_msg.header = data.header
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
pub.publish(imu_msg)
def imu_raw_callback(self, msg):
if self.start_vo:
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.angular_velocity = msg.angular_velocity
imu_msg.angular_velocity_covariance = msg.angular_velocity_covariance
imu_msg.linear_acceleration = msg.linear_acceleration
imu_msg.linear_acceleration_covariance = msg.linear_acceleration_covariance
imu_msg.header.frame_id = "imu"
for i in range(0, 9, 4):
imu_msg.orientation_covariance[i] = IMU_SIGMA**2
self.imupub.publish(imu_msg)
def callbackImu(msg):
# Grab accelerometer and gyro data.
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = 'imu_link'
q = np.array([msg.quaternion.x, msg.quaternion.y, msg.quaternion.z, msg.quaternion.w])
q = normalize(q)
euler = tf.transformations.euler_from_quaternion(q)
q = tf.transformations.quaternion_from_euler(-(euler[0] + math.pi / 2), euler[1], euler[2] - math.pi)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
euler_new = tf.transformations.euler_from_quaternion(q)
rospy.loginfo("euler[0] = %s, euler[1] = %s, euler = [2] = %s",
str(euler_new[0]), str(euler_new[1]), str(euler_new[2]))
imu_msg.orientation_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
imu_msg.angular_velocity = msg.gyro
imu_msg.angular_velocity_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
# Estimate gyro bias.
global gyro_readings
global MAX_GYRO_READINGS
global gyro_bias
if len(gyro_readings) < MAX_GYRO_READINGS:
gyro_readings.append(imu_msg.angular_velocity.z)
elif math.isnan(gyro_bias):
gyro_bias = sum(gyro_readings)/MAX_GYRO_READINGS
if not math.isnan(gyro_bias):
imu_msg.angular_velocity.z -= gyro_bias
imu_msg.linear_acceleration = msg.accelerometer
imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \
0, 0.90, 0, \
0, 0, 0.90]
# Grab magnetometer data.
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.magnetic_field = msg.magnetometer
# TODO(gbrooks): Add covariance.
# Publish sensor data.
imu_pub.publish(imu_msg)
mag_pub.publish(mag_msg)
def callback(raw_data, pub):
imu = Imu()
imu.header = raw_data.header
# set imu.orientation quaternion
# Btw, robot_pose_ekf expects the Roll and Pitch angles to be
# absolute, but the Yaw angle to be relative. Compute the relative
# angle from the currently reported and previously reported absolute
# angle
r = raw_data.roll
p = raw_data.pitch
y = raw_data.yaw - callback.old_yaw
callback.old_yaw = raw_data.yaw
quat = quaternion_from_euler(r,p,y,"sxyz")
imu.orientation.x = quat[0]
imu.orientation.y = quat[1]
imu.orientation.z = quat[2]
imu.orientation.w = quat[3]
#TODO: set imu.orientation_covariance
#row major about x, y, z
# According to OS5000 datasheet, accuracy
# depends on tilt:
# 0.5 deg RMS level heading,
# 1 deg typical RMS accuracy < +-30deg tilt,
# 1.5deg < +-60deg tilt
# Roll and Pitch:
# Typical 1deg accuracy for tilt < +-30 deg
# Assume tilt < +- 30 deg, set all standard
# deviations to 1 degree
std_dev = from_degrees(1)
imu.orientation_covariance[0] = std_dev**2
imu.orientation_covariance[4] = std_dev**2
# standard deviation on yaw is doubled since
# it's computed as a difference of two measurements,
# each with a std_dev of 1 degree
imu.orientation_covariance[8] = (2*std_dev)**2
# no angular velocity data is available, so set
# element 0 of covariance matrix to -1
# alternatively, set the covariance really high
imu.angular_velocity = Vector3(0,0,0)
imu.angular_velocity_covariance[0] = -1
# Set linear acceleration
imu.linear_acceleration = raw_data.acceleration
# TODO: Set linear acceleration covariance
imu.linear_acceleration_covariance[0] = -1
pub.publish(imu)
def callback(raw_data, pub):
imu = Imu()
imu.header = raw_data.header
# set imu.orientation quaternion
# Btw, robot_pose_ekf expects the Roll and Pitch angles to be
# absolute, but the Yaw angle to be relative. Compute the relative
# angle from the currently reported and previously reported absolute
# angle
r = raw_data.roll
p = raw_data.pitch
y = raw_data.yaw - callback.old_yaw
callback.old_yaw = raw_data.yaw
quat = quaternion_from_euler(r, p, y, "sxyz")
imu.orientation.x = quat[0]
imu.orientation.y = quat[1]
imu.orientation.z = quat[2]
imu.orientation.w = quat[3]
#TODO: set imu.orientation_covariance
#row major about x, y, z
# According to OS5000 datasheet, accuracy
# depends on tilt:
# 0.5 deg RMS level heading,
# 1 deg typical RMS accuracy < +-30deg tilt,
# 1.5deg < +-60deg tilt
# Roll and Pitch:
# Typical 1deg accuracy for tilt < +-30 deg
# Assume tilt < +- 30 deg, set all standard
# deviations to 1 degree
std_dev = from_degrees(1)
imu.orientation_covariance[0] = std_dev**2
imu.orientation_covariance[4] = std_dev**2
# standard deviation on yaw is doubled since
# it's computed as a difference of two measurements,
# each with a std_dev of 1 degree
imu.orientation_covariance[8] = (2 * std_dev)**2
# no angular velocity data is available, so set
# element 0 of covariance matrix to -1
# alternatively, set the covariance really high
imu.angular_velocity = Vector3(0, 0, 0)
imu.angular_velocity_covariance[0] = -1
# Set linear acceleration
imu.linear_acceleration = raw_data.acceleration
# TODO: Set linear acceleration covariance
imu.linear_acceleration_covariance[0] = -1
pub.publish(imu)
def convert_to_baselink(self, imu_msg, imu_model, transform):
# convert imu_msg from its frame to baselink frame
# Assumption! TODO: I'm going to assume that the axis are aligned between frames to start
# come back to this later, and rotate to align axis first
# proposed path:
# -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link
# -> run the same code below
'''
[sensor_msgs/Imu]:
std_msgs/Header header - DONE
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration - DONE
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance - DONE
'''
new_msg = Imu()
# Header
new_msg.header = imu_msg.header
new_msg.header.frame_id = '/base_link'
# Orientation (same based on Assumption! above)
new_msg.orientation = imu_msg.orientation
# including covariance, because same. will likely drop for rotation
new_msg.orientation_covariance = imu_msg.orientation_covariance
# Angular Velocity (same based on Assumption! above)
new_msg.angular_velocity = imu_msg.angular_velocity
# including covariance, because same. will likely drop for rotation
new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance
# Linear Acceleration (not the same, even with assumption)
new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform)
return new_msg
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'e'):
self._left_encoder_value = long(lineParts[1])
self._right_encoder_value = long(lineParts[2])
self._Left_Encoder.publish(self._left_encoder_value)
self._Right_Encoder.publish(self._right_encoder_value)
#if(lineParts[0] == 'b'):
#self._battery_value = float(lineParts[1])
#self._Battery_Level.publish(self._battery_value)
if(lineParts[0] == 'u'):
self._ultrasonic_value = float(lineParts[1])
self._Ultrasonic_Value.publish(self._ultrasonic_value)
if(lineParts[0] == 'i'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
self._qx_.publish(self._qx)
self._qy_.publish(self._qy)
self._qz_.publish(self._qz)
self._qw_.publish(self._qw)
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def __call__(self, channel, data):
lcm_msg = agile.imuRaw_t.decode(data)
ros_msg = Imu()
ros_msg.header = self.make_header(lcm_msg.utime)
ros_msg.angular_velocity.x = lcm_msg.gyro[0]
ros_msg.angular_velocity.y = lcm_msg.gyro[1]
ros_msg.angular_velocity.z = lcm_msg.gyro[2]
ros_msg.angular_velocity_covariance = self.angular_velocity_cov
ros_msg.linear_acceleration.x = lcm_msg.accel[0]
ros_msg.linear_acceleration.y = lcm_msg.accel[1]
ros_msg.linear_acceleration.z = lcm_msg.accel[2]
ros_msg.linear_acceleration_covariance = self.linear_acceleration_cov
return [self.rostopic], [ros_msg]
def on_imu_relay(self, msg):
parts = msg.data.split(':')
# Validate type.
typ = parts[0]
assert typ in 'aeg', 'Invalid typ: %s' % typ
# Convert the integers to the original floats.
nums = ltof(parts[1:])
for num, axis in zip(nums, 'xyz'):
self._imu_data['%s%s' % (typ, axis)] = num
# If we've received the final segment, re-publish the complete IMU message.
if typ == 'a':
# https://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
imu_msg = Imu()
imu_msg.header = Header()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = c.BASE_LINK #TODO
# Our sensor returns Euler angles in degrees, but ROS requires radians.
# http://answers.ros.org/question/69754/quaternion-transformations-in-python/
roll = self._imu_data['ex']
pitch = self._imu_data['ey']
yaw = self._imu_data['ez']
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
imu_msg.orientation.x = quaternion[0]
imu_msg.orientation.y = quaternion[1]
imu_msg.orientation.z = quaternion[2]
imu_msg.orientation.w = quaternion[3]
imu_msg.orientation_covariance = [
1, 0.001, 0.001, 0.001, 1, 0.001, 0.001, 0.001, 1
]
imu_msg.angular_velocity.x = self._imu_data['gx']
imu_msg.angular_velocity.y = self._imu_data['gy']
imu_msg.angular_velocity.z = self._imu_data['gz']
imu_msg.angular_velocity_covariance = [
1, 0.001, 0.001, 0.001, 1, 0.001, 0.001, 0.001, 1
]
imu_msg.linear_acceleration.x = self._imu_data['ax']
imu_msg.linear_acceleration.y = self._imu_data['ay']
imu_msg.linear_acceleration.z = self._imu_data['az']
imu_msg.linear_acceleration_covariance = [
1, 0.001, 0.001, 0.001, 1, 0.001, 0.001, 0.001, 1
]
self.imu_pub.publish(imu_msg)
def default(self, ci='unused'):
imu = Imu()
imu.header = self.get_ros_header()
imu.orientation = self.component_instance.bge_object.worldOrientation.to_quaternion()
imu.angular_velocity.x = self.data['angular_velocity'][0]
imu.angular_velocity.y = self.data['angular_velocity'][1]
imu.angular_velocity.z = self.data['angular_velocity'][2]
imu.linear_acceleration.x = self.data['linear_acceleration'][0]
imu.linear_acceleration.y = self.data['linear_acceleration'][1]
imu.linear_acceleration.z = self.data['linear_acceleration'][2]
self.publish(imu)
def map_imu(values):
"""
Map the values generated with the hypothesis-ros imu strategy to a rospy imu type.
"""
if not isinstance(values, _Imu):
raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
ros_imu = Imu()
ros_imu.header = values.header
ros_imu.orientation = values.orientation
ros_imu.orientation_covariance = values.orientation_covariance
ros_imu.angular_velocity = values.angular_velocity
ros_imu.angular_velocity_covariance = values.angular_velocity_covariance
ros_imu.linear_acceleration = values.linear_acceleration
ros_imu.linear_acceleration_covariance = values.linear_acceleration_covariance
return ros_imu
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
while len(line) > 0:
lineParts = line.split("\t")
try:
if lineParts[0] == "e":
self.angle_z = float(lineParts[1])
self._qx = 0.0
self._qy = 0.0
self._qz = math.sin(math.pi * self.angle_z / 360.0)
self._qw = math.cos(math.pi * self.angle_z / 360.0)
#######################################################################################################################
self._qx_.publish(self._qx)
self._qy_.publish(self._qy)
self._qz_.publish(self._qz)
self._qw_.publish(self._qw)
#######################################################################################################################
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1.0,) * 9
imu_msg.angular_velocity_covariance = (-1.0,) * 9
imu_msg.linear_acceleration_covariance = (-1.0,) * 9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def default(self, ci='unused'):
""" Publish the data of the IMU sensor as a custom ROS Imu message """
imu = Imu()
imu.header = self.get_ros_header()
imu.orientation = self.orientation
imu.angular_velocity.x = self.data['angular_velocity'][0]
imu.angular_velocity.y = self.data['angular_velocity'][1]
imu.angular_velocity.z = self.data['angular_velocity'][2]
imu.linear_acceleration.x = self.data['linear_acceleration'][0]
imu.linear_acceleration.y = self.data['linear_acceleration'][1]
imu.linear_acceleration.z = self.data['linear_acceleration'][2]
self.publish(imu)
def subCB(msg_in):
global pub
msg_out = Imu()
msg_out.header = msg_in.header
q = quaternion_from_euler(msg_in.RPY.x/180.0 * pi,
msg_in.RPY.y/180.0 * pi,
msg_in.RPY.z/180.0 * pi)
msg_out.orientation.x = q[0]
msg_out.orientation.y = q[1]
msg_out.orientation.z = q[2]
msg_out.orientation.w = q[3]
pub.publish(msg_out)
def Handle_Received_Line(self, line):
self._counter = self._counter + 1
self.SerialPublisher.publish(String(str(self._counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'e'):
self._left_encoder_val = long(lineParts[1])
self._right_encoder_val = long(lineParts[2])
self.left_encoder_pub.publish(self._left_encoder_val)
self.right_encoder_pub.publish(self._right_encoder_val)
if(lineParts[0] == 'u'):
self._ultrasonic_val = float(lineParts[1])
self.ultrasonic_pub.publish(self._ultrasonic_val)
if(lineParts[0] == 'i'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
self._qx_pub.publish(self._qx)
self._qy_pub.publish(self._qy)
self._qz_pub.publish(self._qz)
self._qw_pub.publish(self._qw)
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in the sensor values")
rospy.logwarn(lineParts)
pass
def imuDataReceived(data):
global imu_repub
angles = tf.transformations.euler_from_quaternion(
[data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w]
)
imu_msg = Imu()
yaw = wrapToPi(angles[2] - math.pi / 2)
orientation = tf.transformations.quaternion_from_euler(0, 0, yaw)
imu_msg.orientation.x = orientation[0]
imu_msg.orientation.y = orientation[1]
imu_msg.orientation.z = orientation[2]
imu_msg.orientation.w = orientation[3]
imu_msg.header = data.header
imu_msg.orientation_covariance = data.orientation_covariance
imu_msg.angular_velocity = data.angular_velocity
imu_repub.publish(imu_msg)
def magnetic_cb(msg):
global x_max, x_min, y_max, y_min, x_center, y_center, x_scale, y_scale
if autocompute:
x_max = max(x_max, msg.vector.x)
x_min = min(x_min, msg.vector.x)
y_max = max(y_max, msg.vector.y)
y_min = min(y_min, msg.vector.y)
x_center = (x_max + x_min) / 2.0
y_center = (y_max + y_min) / 2.0
x_scale = x_max - x_min
y_scale = y_max - y_min
x = (msg.vector.x - x_center) / x_scale
y = (msg.vector.y - y_center) / y_scale
heading = atan2(x, y)
imu = Imu()
imu.header = msg.header
imu.header.frame_id = 'odom'
q = tf.transformations.quaternion_from_euler(0, 0, heading)
imu.orientation = Quaternion(*q)
imu.orientation_covariance = [ compass_var, 0.0, 0.0,
0.0, compass_var, 0.0,
0.0, 0.0, compass_var ]
imu.angular_velocity_covariance = [ -1, 0, 0,
0, 0, 0,
0, 0, 0 ]
imu.linear_acceleration_covariance = [ -1, 0, 0,
0, 0, 0,
0, 0, 0 ]
imu_pub.publish(imu)
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if(len(line) > 0):
lineParts = line.split('\t')
try:
if(lineParts[0] == 'yaw'):
self._ax = float(lineParts[1])
yaw_msg = Float64()
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
yaw_msg.data = self._ax
q = quaternion_from_euler(0,0,self._ax*-degrees2rad)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
imu_msg.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6]
imu_msg.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
self.imu_pub.publish(imu_msg)
self.yaw_pub.publish(yaw_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def run(self):
counter = 0
rospy.loginfo("Simulation started")
while not rospy.is_shutdown():
self.q_pub.publish(Quaternion(self.theta, self.omega, self.bias_yaw, self.delta_mag))
now = rospy.Time.now()
rpy = Vector3Stamped()
rpy.header.stamp = now
rpy.vector.z = self.theta - self.bias_yaw + self.noise(self.noise_yaw)
self.rpy_pub.publish(rpy)
mag = Vector3Stamped()
mag.header = rpy.header
magnitude = 150 + self.noise(self.noise_magnitude_mag)
theta = self.theta - self.delta_mag + self.noise(self.noise_mag)
mag.vector.x = magnitude * math.cos(theta)
mag.vector.y = magnitude * math.sin(theta)
self.mag_pub.publish(mag)
imu = Imu()
imu.header = mag.header
imu.angular_velocity.z = self.omega + self.noise(self.noise_omega)
self.imu_pub.publish(imu)
if (counter % 20) == 0:
gps = GPSFix()
gps.header = rpy.header
gps.speed = 1.0
gps.track = self.theta + self.noise(self.noise_gps)
gps.track = norm_angle(gps.track)
self.gps_pub.publish(gps)
counter += 1
self.bias_index += self.bias_speed * self.dt
self.bias_yaw = 2 * math.pi * math.cos(self.bias_index)
self.theta += self.omega * self.dt
self.theta = norm_angle(self.theta)
rospy.sleep(self.dt)
def talker():
"""Initializes the publisher node"""
global pub
pub = rospy.Publisher("mpu6050", Imu)
rospy.init_node("MPU6050-Driver")
# calibrate(imu, accel+gyro, samples = 0)
global seq
seq = 0
while not rospy.is_shutdown():
sample = Imu()
sample.header = make_header()
sample.orientation_covariance[0] = -1
sample.angular_velocity_covariance = [0] * 9
sample.angular_velocity = read_gyros()
sample.linear_acceleration_covariance = [0] * 9
sample.linear_acceleration = read_accels()
rospy.loginfo(str(sample))
pub.publish(sample)
time.sleep(0.1)
def velodyne_Dat():
nmea = rospy.Publisher('nmea_sentence', Sentence, queue_size=1)
imu = rospy.Publisher('imu_data', Imu, queue_size=1)
rospy.init_node('velodyne_GPS', anonymous=True)
r = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
data, addr = sock.recvfrom(1206) # buffer size is 1206 bytes
if (addr[0] == "192.168.1.201"): # Ensure message is from Velodyne HDL32-E
val = data[206:278].encode('ascii')
gyro1_temp =(data[15:16] + data[14:15]).encode("hex")
gyro1 = int( gyro1_temp[1:],16)
if gyro1 > 2047:
gyro1 = (-4096 + gyro1)
gyro1 = gyro1 * GYRO_SCALE_FACTOR
temp1_temp =(data[17:18] + data[16:17]).encode("hex")
temp1 = int( temp1_temp[1:],16)
if temp1 > 2047:
temp1 = (-4096 + temp1)
temp1 = (temp1 * TEMP_SCALE_FACTOR) + TEMP_SUM_FACTOR
accel1X_temp =(data[19:20] + data[18:19]).encode("hex")
accel1X = int( accel1X_temp[1:],16)
if accel1X > 2047:
accel1X = (-4096 + accel1X)
accel1X = accel1X * ACCEL_SCALE_FACTOR
accel1Y_temp =(data[21:22] + data[20:21]).encode("hex")
accel1Y = int( accel1Y_temp[1:],16)
if accel1Y > 2047:
accel1Y = (-4096 + accel1Y)
accel1Y = accel1Y * ACCEL_SCALE_FACTOR
gyro2_temp =(data[23:24] + data[22:23]).encode("hex")
gyro2 = int( gyro2_temp[1:],16)
if gyro2 > 2047:
gyro2 = (-4096 + gyro2)
gyro2 = gyro2 * GYRO_SCALE_FACTOR
temp2_temp =(data[25:26] + data[24:25]).encode("hex")
temp2 = int( temp2_temp[1:],16)
if temp2 > 2047:
temp2 = (-4096 + temp2)
temp2 = (temp2 * TEMP_SCALE_FACTOR) + TEMP_SUM_FACTOR
print "temp2 ",temp2
accel2X_temp =(data[27:28] + data[26:27]).encode("hex")
accel2X = int( accel2X_temp[1:],16)
if accel2X > 2047:
accel2X = (-4096 + accel2X)
accel2X = accel2X * ACCEL_SCALE_FACTOR
accel2Y_temp =(data[29:30] + data[28:29]).encode("hex")
accel2Y = int( accel2Y_temp[1:],16)
if accel2Y > 2047:
accel2Y = (-4096 + accel2Y)
accel2Y = accel2Y * ACCEL_SCALE_FACTOR
gyro3_temp =(data[31:32] + data[30:31]).encode("hex")
gyro3 = int( gyro3_temp[1:],16)
if gyro3 > 2047:
gyro3 = (-4096 + gyro3)
gyro3 = gyro3 * GYRO_SCALE_FACTOR
temp3_temp =(data[33:34] + data[32:33]).encode("hex")
temp3 = int( temp3_temp[1:],16)
if temp3 > 2047:
temp3 = (-4096 + temp3)
temp3 = (temp3 * TEMP_SCALE_FACTOR) + TEMP_SUM_FACTOR
accel3X_temp =(data[35:36] + data[34:35]).encode("hex")
accel3X = int( accel3X_temp[1:],16)
if accel3X > 2047:
accel3X = (-4096 + accel3X)
accel3X = accel3X * ACCEL_SCALE_FACTOR
accel3Y_temp =(data[37:38] + data[36:37]).encode("hex")
accel3Y = int( accel3Y_temp[1:],16)
if accel3Y > 2047:
accel3Y = (-4096 + accel3Y)
accel3Y = accel3Y * ACCEL_SCALE_FACTOR
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = "velodyne"
Sen = nmea_msgs.msg.Sentence()
Sen.header = h
Sen.sentence = val # "$GPRMC,192326,A,4324.2427,N,08028.2217,W,000.0,000.0,080914,009.7,W,D*1F"
imu_msg = Imu();
imu_msg.header = h
imu_msg.linear_acceleration.x = ((accel1X + accel2X) / 2) * CONVERT_MSEC2
imu_msg.linear_acceleration.y = ((accel2Y + accel3Y) / 2) * CONVERT_MSEC2
imu_msg.linear_acceleration.z = ((accel3X + (-accel1Y)) / 2) * CONVERT_MSEC2
imu_msg.angular_velocity.x = -gyro3 * CONVERT_RADSEC
imu_msg.angular_velocity.y = gyro1 * CONVERT_RADSEC
imu_msg.angular_velocity.z = gyro2 * CONVERT_RADSEC
imu.publish(imu_msg)
nmea.publish(Sen)
r.sleep()
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if len(line) > 0:
lineParts = line.split("\t")
try:
if lineParts[0] == "quat":
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
if lineParts[0] == "ypr":
self._ax = float(lineParts[1])
self._ay = float(lineParts[2])
self._az = float(lineParts[3])
if lineParts[0] == "areal":
self._lx = float(lineParts[1])
self._ly = float(lineParts[2])
self._lz = float(lineParts[3])
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
# imu_msg.orientation_covariance = (1., )*9
imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
# imu_msg.angular_velocity_covariance = (1., )*9
imu_msg.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6]
# imu_msg.linear_acceleration_covariance = (1., )*9
imu_msg.linear_acceleration_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0]
imu_msg.orientation.x = self._qx
# imu_msg.orientation.x = 0
imu_msg.orientation.y = self._qy
# imu_msg.orientation.y = 0
imu_msg.orientation.z = self._qz
# imu_msg.orientation.z = 0
imu_msg.orientation.w = self._qw
# imu_msg.orientation.w = 1
imu_msg.angular_velocity.x = self._ax
# imu_msg.angular_velocity.x = 0
imu_msg.angular_velocity.y = self._ay
# imu_msg.angular_velocity.y = 0
imu_msg.angular_velocity.z = self._az
# imu_msg.angular_velocity.z = 0
# imu_msg.linear_acceleration.x = self._lx
imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = self._ly
imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = self._lz
imu_msg.linear_acceleration.z = 0
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
try:
imu_data = self.zumy.read_imu()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
except ValueError:
pass
try:
enc_data = self.zumy.enc_pos()
enc_msg = Float32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
except ValueError:
pass
try:
vel_data = self.zumy.enc_vel()
vel_msg = Float32()
vel_msg.data = vel_data[0]
self.l_vel_pub.publish(vel_msg)
vel_msg.data = vel_data[1]
self.r_vel_pub.publish(vel_msg)
except ValueError:
pass
try:
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
except ValueError:
pass
if time.time() > (self.last_message_at + self.timeout): #i've gone too long without seeing the watchdog.
self.watchdog = False
self.zumy.disable()
self.zumy.battery_protection() # a function that needs to be called with some regularity.
#handle the robot status message
status_msg = ZumyStatus()
status_msg.enabled_status = self.zumy.enabled
status_msg.battery_unsafe = self.zumy.battery_unsafe()
#update the looptimes, which will get published in status_msg
self.laptimes = np.delete(self.laptimes,0)
self.laptimes = np.append(self.laptimes,time.time())
#take average over the difference of the times... and then invert. That will give you average freq.
status_msg.loop_freq = 1/float(np.mean(np.diff(self.laptimes)))
self.status_pub.publish(status_msg)
self.lock.release() #must be last command involving the zumy.
self.rate.sleep()
# If shutdown, turn off motors & disable anything else.
self.zumy.disable()
def spin_once(self):
'''Read data from device and publishes ROS messages.'''
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
press_msg = Float32()
pub_press = False
anin1_msg = UInt16()
pub_anin1 = False
anin2_msg = UInt16()
pub_anin2 = False
pub_diag = False
def fill_from_raw(raw_data):
'''Fill messages with information from 'raw' MTData block.'''
# don't publish raw imu data anymore
# TODO find what to do with that
pass
def fill_from_rawgps(rawgps_data):
'''Fill messages with information from 'rawgps' MTData block.'''
global pub_hps, xgps_msg, gps_msg
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
def fill_from_Temp(temp):
'''Fill messages with information from 'temperature' MTData block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = temp
def fill_from_Calib(imu_data):
'''Fill messages with information from 'calibrated' MTData block.'''
global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg
try:
pub_imu = True
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_vel = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
except KeyError:
pass
try:
pub_imu = True
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
except KeyError:
pass
try:
pub_mag = True
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
except KeyError:
pass
def fill_from_Vel(velocity_data):
'''Fill messages with information from 'velocity' MTData block.'''
global pub_vel, vel_msg
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.'''
global pub_imu, imu_msg
pub_imu = True
if orient.has_key('quaternion'):
w, x, y, z = orient['quaternion']
elif orient.has_key('roll'):
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(orient['roll']),
radians(orient['pitch']), radians(orient['yaw']))
elif orient.has_key('matrix'):
m = identity_matrix()
m[:3,:3] = orient['matrix']
x, y, z, w = quaternion_from_matrix(m)
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Pos(position_data):
'''Fill messages with information from 'position' MTData block.'''
global pub_gps, xgps_msg, gps_msg
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
def fill_from_Stat(status):
'''Fill messages with information from 'status' MTData block.'''
global pub_diag, pub_gps, gps_msg, xgps_msg
pub_diag = True
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
def fill_from_Auxiliary(aux_data):
'''Fill messages with information from 'Auxiliary' MTData block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['Ain_1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['Ain_2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Temperature(o):
'''Fill messages with information from 'Temperature' MTData2 block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = o['Temp']
def fill_from_Timestamp(o):
'''Fill messages with information from 'Timestamp' MTData2 block.'''
global h
try:
# put timestamp from gps UTC time if available
y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
if f&0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO find what to do with other kind of information
pass
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']), radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Pressure(o):
'''Fill messages with information from 'Pressure' MTData2 block.'''
global pub_press, press_msg
press_msg.data = o['Pressure']
def fill_from_Acceleration(o):
'''Fill messages with information from 'Acceleration' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
# FIXME not sure we should treat all in that same way
try:
x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
except KeyError:
pass
try:
x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
except KeyError:
pass
try:
x, y, z = o['accX'], o['accY'], o['accZ']
except KeyError:
pass
imu_msg.linear_acceleration.x = x
imu_msg.linear_acceleration.y = y
imu_msg.linear_acceleration.z = z
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
def fill_from_Position(o):
'''Fill messages with information from 'Position' MTData2 block.'''
global pub_gps, xgps_msg, gps_msg
# TODO publish ECEF
try:
xgps_msg.latitude = gps_msg.latitude = o['lat']
xgps_msg.longitude = gps_msg.longitude = o['lon']
pub_gps = True
alt = o.get('altMsl', o.get('altEllipsoid', 0))
xgps_msg.altitude = gps_msg.altitude = alt
except KeyError:
pass
def fill_from_Angular_Velocity(o):
'''Fill messages with information from 'Angular Velocity' MTData2 block.'''
global pub_imu, imu_msg, pub_vel, vel_msg
try:
imu_msg.angular_velocity.x = o['gyrX']
imu_msg.angular_velocity.y = o['gyrY']
imu_msg.angular_velocity.z = o['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = o['gyrX']
vel_msg.twist.angular.y = o['gyrY']
vel_msg.twist.angular.z = o['gyrZ']
pub_vel = True
except KeyError:
pass
# TODO decide what to do with 'Delta q'
def fill_from_GPS(o):
'''Fill messages with information from 'GPS' MTData2 block.'''
global pub_gps, h, xgps_msg
try: # DOP
xgps_msg.gdop = o['gDOP']
xgps_msg.pdop = o['pDOP']
xgps_msg.hdop = o['hDOP']
xgps_msg.vdop = o['vDOP']
xgps_msg.tdop = o['tDOP']
pub_gps = True
except KeyError:
pass
try: # Time UTC
y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\
o['hour'], o['min'], o['sec'], o['nano'], o['valid']
if f&0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO publish SV Info
def fill_from_SCR(o):
'''Fill messages with information from 'SCR' MTData2 block.'''
# TODO that's raw information
pass
def fill_from_Analog_In(o):
'''Fill messages with information from 'Analog In' MTData2 block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['analogIn1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['analogIn2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Magnetic(o):
'''Fill messages with information from 'Magnetic' MTData2 block.'''
global pub_mag, mag_msg
mag_msg.vector.x = o['magX']
mag_msg.vector.y = o['magY']
mag_msg.vector.z = o['magZ']
pub_mag = True
def fill_from_Velocity(o):
'''Fill messages with information from 'Velocity' MTData2 block.'''
global pub_vel, vel_msg
vel_msg.twist.linear.x = o['velX']
vel_msg.twist.linear.y = o['velY']
vel_msg.twist.linear.z = o['velZ']
pub_vel = True
def fill_from_Status(o):
'''Fill messages with information from 'Status' MTData2 block.'''
try:
status = o['StatusByte']
fill_from_Stat(status)
except KeyError:
pass
try:
status = o['StatusWord']
fill_from_Stat(status)
except KeyError:
pass
# TODO RSSI
def find_handler_name(name):
return "fill_from_%s"%(name.replace(" ", "_"))
# get data
data = self.mt.read_measurement()
# fill messages based on available data fields
for n, o in data:
try:
locals()[find_handler_name(n)](o)
except KeyError:
rospy.logwarn("Unknown MTi data packet: '%s', ignoring."%n)
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
if pub_press:
self.press_pub.publish(press_msg)
if pub_anin1:
self.analog_in1_pub.publish(anin1_msg)
if pub_anin2:
self.analog_in2_pub.publish(anin2_msg)
if pub_diag:
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
# publish string representation
self.str_pub.publish(str(data))
def spin_once(self):
def baroPressureToHeight(value):
c1 = 44330.0
c2 = 9.869232667160128300024673081668e-6
c3 = 0.1901975534856
intermediate = 1-math.pow(c2*value, c3)
height = c1*intermediate
return height
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
#temp = data.get('Temp') # float
orient_data = data.get('Orientation Data')
velocity_data = data.get('Velocity')
position_data = data.get('Latlon')
altitude_data = data.get('Altitude')
acc_data = data.get('Acceleration')
gyr_data = data.get('Angular Velocity')
mag_data = data.get('Magnetic')
pressure_data = data.get('Pressure')
time_data = data.get('Timestamp')
gnss_data = data.get('Gnss PVT')
status = data.get('Status') # int
# create messages and default values
"Imu message supported with Modes 1 & 2"
imu_msg = Imu()
pub_imu = False
"SensorSample message supported with Mode 2"
ss_msg = sensorSample()
pub_ss = False
"Mag message supported with Modes 1 & 2"
mag_msg = Vector3Stamped()
pub_mag = False
"Baro in meters"
baro_msg = baroSample()
pub_baro = False
"GNSS message supported only with MTi-G-7xx devices"
"Valid only for modes 1 and 2"
gnssPvt_msg = gnssSample()
pub_gnssPvt = False
#gnssSatinfo_msg = GPSStatus()
#pub_gnssSatinfo = False
# All filter related outputs
"Supported in mode 3"
ori_msg = orientationEstimate()
pub_ori = False
"Supported in mode 3 for MTi-G-7xx devices"
vel_msg = velocityEstimate()
pub_vel = False
"Supported in mode 3 for MTi-G-7xx devices"
pos_msg = positionEstimate()
pub_pos = False
secs = 0
nsecs = 0
if time_data:
# first getting the sampleTimeFine
time = time_data['SampleTimeFine']
secs = 100e-6*time
nsecs = 1e5*time - 1e9*math.floor(secs)
if acc_data:
if 'Delta v.x' in acc_data: # found delta-v's
pub_ss = True
ss_msg.internal.imu.dv.x = acc_data['Delta v.x']
ss_msg.internal.imu.dv.y = acc_data['Delta v.y']
ss_msg.internal.imu.dv.z = acc_data['Delta v.z']
elif 'accX' in acc_data: # found acceleration
pub_imu = True
imu_msg.linear_acceleration.x = acc_data['accX']
imu_msg.linear_acceleration.y = acc_data['accY']
imu_msg.linear_acceleration.z = acc_data['accZ']
else:
raise MTException("Unsupported message in XDI_AccelerationGroup.")
if gyr_data:
if 'Delta q0' in gyr_data: # found delta-q's
pub_ss = True
ss_msg.internal.imu.dq.w = gyr_data['Delta q0']
ss_msg.internal.imu.dq.x = gyr_data['Delta q1']
ss_msg.internal.imu.dq.y = gyr_data['Delta q2']
ss_msg.internal.imu.dq.z = gyr_data['Delta q3']
elif 'gyrX' in gyr_data: # found rate of turn
pub_imu = True
imu_msg.angular_velocity.x = gyr_data['gyrX']
imu_msg.angular_velocity.y = gyr_data['gyrY']
imu_msg.angular_velocity.z = gyr_data['gyrZ']
else:
raise MTException("Unsupported message in XDI_AngularVelocityGroup.")
if mag_data:
# magfield
ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX']
ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY']
ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ']
pub_mag = True
if pressure_data:
pub_baro = True
height = baroPressureToHeight(pressure_data['Pressure'])
baro_msg.height = ss_msg.internal.baro.height = height
if gnss_data:
pub_gnssPvt = True
gnssPvt_msg.itow = gnss_data['iTOW']
gnssPvt_msg.fix = gnss_data['fix']
gnssPvt_msg.latitude = gnss_data['lat']
gnssPvt_msg.longitude = gnss_data['lon']
gnssPvt_msg.hEll = gnss_data['hEll']
gnssPvt_msg.hMsl = gnss_data['hMsl']
gnssPvt_msg.vel.x = gnss_data['velE']
gnssPvt_msg.vel.y = gnss_data['velN']
gnssPvt_msg.vel.z = -gnss_data['velD']
gnssPvt_msg.hAcc = gnss_data['horzAcc']
gnssPvt_msg.vAcc = gnss_data['vertAcc']
gnssPvt_msg.sAcc = gnss_data['speedAcc']
gnssPvt_msg.pDop = gnss_data['PDOP']
gnssPvt_msg.hDop = gnss_data['HDOP']
gnssPvt_msg.vDop = gnss_data['VDOP']
gnssPvt_msg.numSat = gnss_data['nSat']
gnssPvt_msg.heading = gnss_data['heading']
gnssPvt_msg.headingAcc = gnss_data['headingAcc']
if orient_data:
if 'Q0' in orient_data:
pub_imu = True
imu_msg.orientation.x = orient_data['Q0']
imu_msg.orientation.y = orient_data['Q1']
imu_msg.orientation.z = orient_data['Q2']
imu_msg.orientation.w = orient_data['Q3']
elif 'Roll' in orient_data:
pub_ori = True
ori_msg.roll = orient_data['Roll']
ori_msg.pitch = orient_data['Pitch']
ori_msg.yaw = orient_data['Yaw']
else:
raise MTException('Unsupported message in XDI_OrientationGroup')
if velocity_data:
pub_vel = True
vel_msg.velE = velocity_data['velX']
vel_msg.velN = velocity_data['velY']
vel_msg.velU = velocity_data['velZ']
if position_data:
pub_pos = True
pos_msg.latitude = position_data['lat']
pos_msg.longitude = position_data['lon']
if altitude_data:
pub_pos = True
tempData = altitude_data['ellipsoid']
pos_msg.hEll = tempData[0]
#if status is not None:
# if status & 0b0001:
# self.stest_stat.level = DiagnosticStatus.OK
# self.stest_stat.message = "Ok"
# else:
# self.stest_stat.level = DiagnosticStatus.ERROR
# self.stest_stat.message = "Failed"
# if status & 0b0010:
# self.xkf_stat.level = DiagnosticStatus.OK
# self.xkf_stat.message = "Valid"
# else:
# self.xkf_stat.level = DiagnosticStatus.WARN
# self.xkf_stat.message = "Invalid"
# if status & 0b0100:
# self.gps_stat.level = DiagnosticStatus.OK
# self.gps_stat.message = "Ok"
# else:
# self.gps_stat.level = DiagnosticStatus.WARN
# self.gps_stat.message = "No fix"
# self.diag_msg.header = h
# self.diag_pub.publish(self.diag_msg)
# publish available information
if pub_imu:
imu_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
imu_msg.header.stamp.secs = secs
imu_msg.header.stamp.nsecs = nsecs
self.imu_pub.publish(imu_msg)
#if pub_gps:
# xgps_msg.header = gps_msg.header = h
# self.gps_pub.publish(gps_msg)
# self.xgps_pub.publish(xgps_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
#if pub_temp:
# self.temp_pub.publish(temp_msg)
if pub_ss:
ss_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
ss_msg.header.stamp.secs = secs
ss_msg.header.stamp.nsecs = nsecs
self.ss_pub.publish(ss_msg)
if pub_baro:
baro_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
baro_msg.header.stamp.secs = secs
baro_msg.header.stamp.nsecs = nsecs
self.baro_pub.publish(baro_msg)
if pub_gnssPvt:
gnssPvt_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
baro_msg.header.stamp.secs = secs
baro_msg.header.stamp.nsecs = nsecs
self.gnssPvt_pub.publish(gnssPvt_msg)
if pub_ori:
ori_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
ori_msg.header.stamp.secs = secs
ori_msg.header.stamp.nsecs = nsecs
self.ori_pub.publish(ori_msg)
if pub_vel:
vel_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
vel_msg.header.stamp.secs = secs
vel_msg.header.stamp.nsecs = nsecs
self.vel_pub.publish(vel_msg)
if pub_pos:
pos_msg.header = h
#all time assignments (overwriting ROS time)
# Comment the two lines below if you need ROS time
pos_msg.header.stamp.secs = secs
pos_msg.header.stamp.nsecs = nsecs
self.pos_pub.publish(pos_msg)
time = shifted_header.stamp.nsecs + shifted_header.stamp.secs*1e9
# time += (0.0150117606625+0.000657665377756+0.00119533281712-0.000487851613732)*1e9
shifted_header.stamp.nsecs = time % 1e9
shifted_header.stamp.secs = int(time/1e9)
left_image.header = shifted_header
right_image = msg.right_image
right_image.header = msg.header
out_bag.write('/duo3d_camera/left/image_raw', msg.left_image, t)
out_bag.write('/duo3d_camera/right/image_raw', msg.right_image, t)
imu_msg = Imu()
imu_msg.angular_velocity.x = msg.imu.angular_velocity.x
imu_msg.angular_velocity.y = -msg.imu.angular_velocity.y
imu_msg.angular_velocity.z = msg.imu.angular_velocity.z
imu_msg.linear_acceleration.x = msg.imu.linear_acceleration.x * 9.81
imu_msg.linear_acceleration.y = -msg.imu.linear_acceleration.y * 9.81
imu_msg.linear_acceleration.z = -msg.imu.linear_acceleration.z * 9.81
imu_msg.header = msg.header
out_bag.write('/duo3d_camera/cam_imu', imu_msg, t)
imu_cnt += 1
print('\nDone. Output bag:')
print(out_bag)
in_bag.close()
out_bag.close()
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi*orient['roll']/180.,
pi*orient['pitch']/180, pi*orient['yaw']/180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3,:3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Velocity')
position_data = data.get('Position')
rawgps_data = data.get('RAWGPS')
status = data.get('Status') # int
sample = data.get('Sample')
# TODO by @demmeln: use sample counter for timestamp
# correction. Watch out for counter wrap.
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., )*9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., )*9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x&0x8000:
temp_msg.data = (x - 1<<16)/256.
else:
temp_msg.data = x/256.
if rawgps_data:
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
pub_vel = True
except KeyError:
pass
try:
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
pub_imu = True
except KeyError:
pass
try:
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
