def _create_msg(self, data):
"""
Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down
"""
msg1 = Imu()
msg1_magfield = MagneticField()
msg1.header.stamp = rospy.Time.now()
msg1.header.frame_id = '/base_link'
msg1_magfield.header.stamp = rospy.Time.now()
msg1_magfield.header.frame_id = '/base_link'
msg1.linear_acceleration.x = -data["IMU1"]["accel_y"]
msg1.linear_acceleration.y = -data["IMU1"]["accel_z"]
msg1.linear_acceleration.z = data["IMU1"]["accel_x"]
msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.angular_velocity.x = -data["IMU1"]["gyro_y"]
msg1.angular_velocity.y = -data["IMU1"]["gyro_z"]
msg1.angular_velocity.z = data["IMU1"]["gyro_x"]
msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1.orientation.w = 0
msg1.orientation.x = 0
msg1.orientation.y = 0
msg1.orientation.z = 0
msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg1_magfield.magnetic_field.x = -data["IMU1"]["mag_y"]
msg1_magfield.magnetic_field.y = -data["IMU1"]["mag_z"]
msg1_magfield.magnetic_field.z = data["IMU1"]["mag_x"]
msg2 = Imu()
msg2_magfield = MagneticField()
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = '/base_link'
msg2_magfield.header.stamp = rospy.Time.now()
msg2_magfield.header.frame_id = '/base_link'
msg2.linear_acceleration.x = data["IMU2"]["accel_y"]
msg2.linear_acceleration.y = data["IMU2"]["accel_z"]
msg2.linear_acceleration.z = data["IMU2"]["accel_x"]
msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.angular_velocity.x = data["IMU2"]["gyro_y"]
msg2.angular_velocity.y = data["IMU2"]["gyro_z"]
msg2.angular_velocity.z = data["IMU2"]["gyro_x"]
msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2.orientation.w = 0
msg2.orientation.x = 0
msg2.orientation.y = 0
msg2.orientation.z = 0
msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg2_magfield.magnetic_field.x = data["IMU2"]["mag_y"]
msg2_magfield.magnetic_field.y = data["IMU2"]["mag_z"]
msg2_magfield.magnetic_field.z = data["IMU2"]["mag_x"]
return msg1, msg1_magfield, msg2, msg2_magfield
def rawimu_handler(self, rawimus):
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self.base_frame
# Populate orientation field with one from inspvax message.
imu.orientation = Quaternion(*self.orientation)
imu.orientation_covariance = self.orientation_covariance
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = rawimus.x_gyro * 0.1 / (3600.0 *
256.0) * self.imu_rate
imu.angular_velocity.y = -rawimus.my_gyro * 0.1 / (
3600.0 * 256.0) * self.imu_rate
imu.angular_velocity.z = rawimus.z_gyro * 0.1 / (3600.0 *
256.0) * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = rawimus.x_accel * 0.05 / 2**15 * self.imu_rate
imu.linear_acceleration.y = rawimus.my_accel * 0.05 / 2**15 * self.imu_rate
imu.linear_acceleration.z = rawimus.z_accel * 0.05 / 2**15 * self.imu_rate
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
def corrimudata_handler(self, corrimudata):
# TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
imu = Imu()
imu.header.stamp = rospy.Time.now()
# imu.header.frame_id = self.base_frame
imu.header.frame_id = 'imu_link'
# Populate orientation field with one from inspvax message.
imu.orientation = Quaternion(*self.orientation)
imu.orientation_covariance = self.orientation_covariance
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate
imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate
imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate
imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate
imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate + 9.7803
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
def fake_imu():
pub = rospy.Publisher('IMU_ned', Imu, queue_size=10)
rospy.init_node('test_convertimu', anonymous=True)
rate = rospy.Rate(0.5) # 10hz
while not rospy.is_shutdown():
imu_message=Imu()
imu_message.header.stamp=rospy.Time.now()
imu_message.header.frame_id="IMU"
imu_message.orientation.x=1
imu_message.orientation.y=2
imu_message.orientation.z=3
imu_message.orientation.w=4
imu_message.orientation_covariance=[-1,0,0,0,-1,0,0,0,-1]
imu_message.linear_acceleration.x=6
imu_message.linear_acceleration.y=7
imu_message.linear_acceleration.z=8
imu_message.linear_acceleration_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]
imu_message.angular_velocity.x=10
imu_message.angular_velocity.y=11
imu_message.angular_velocity.z=12
imu_message.angular_velocity_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]
pub.publish(imu_message)
rate.sleep()
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 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 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_sensor_data(self, sensor_data):
acc = np.array(sensor_data.acceleration.data) / 1000.0 * g
ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi
# ang_vel = rotate_v(ang_vel.tolist(), self.rotation)
msg = Imu()
msg.header.frame_id = self.base_frame_id
msg.header.stamp = rospy.Time.now()
msg.linear_acceleration = Vector3(*acc)
# It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame
q = sensor_data.quaternion.data
q = [q[1], q[2], q[3], q[0]]
msg.orientation = Quaternion(*q)
# msg.orientation = Quaternion(*self.orientation)
msg.angular_velocity = Vector3(*ang_vel)
msg.linear_acceleration_covariance = self.linear_acceleration_cov
msg.orientation_covariance = self.orientation_cov
msg.angular_velocity_covariance = self.angular_velocity_cov
self.imu_pub.publish(msg)
mag = np.array(sensor_data.magnetic.data) * 1e-6
m_msg = MagneticField()
m_msg.header = msg.header
m_msg.magnetic_field = Vector3(*mag)
m_msg.magnetic_field_covariance = self.magnetic_field_cov
self.mag_pub.publish(m_msg)
def cb_all_data(self, acceleration, magnetic_field, angular_velocity,
euler_angle, quaternion, linear_acceleration,
gravity_vector, temperature, calibration_status):
msg = Imu()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "base_imu_link"
msg.orientation.x = quaternion[1] / 16383.0
msg.orientation.y = quaternion[2] / 16383.0
msg.orientation.z = quaternion[3] / 16383.0
msg.orientation.w = quaternion[0] / 16383.0
# Observed orientation variance: 0.0 (10k samples)
# Magnometer heading accuracy is +-2.5 deg => 0.088 rad
# With heading accuracy as std dev, variance = 0.088^2 = 0.008
msg.orientation_covariance = [0.008, 0, 0, 0, 0.008, 0, 0, 0, 0.008]
msg.angular_velocity.x = angular_velocity[0] / 16.0 * pi / 180
msg.angular_velocity.y = angular_velocity[1] / 16.0 * pi / 180
msg.angular_velocity.z = angular_velocity[2] / 16.0 * pi / 180
# Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
msg.angular_velocity_covariance = [0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02]
msg.linear_acceleration.x = acceleration[0] / 100.0
msg.linear_acceleration.y = acceleration[1] / 100.0
msg.linear_acceleration.z = acceleration[2] / 100.0
# Observed linear acceleration variance: 0.001532 (10k samples)
# Calculation for variance taken from razor imu:
# nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
msg.linear_acceleration_covariance = [
0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04
]
self.pub_imu.publish(msg)
def createMessage(self):
xQ, yQ, zQ, wQ = self.toQuaternion(self.degrees)
deltaAngle = math.radians(float(self.degrees - self.lastDegrees))
deltaTime = float(self.lastMeasurementTime - time())
wz = deltaAngle / deltaTime
self.lastMeasurementTime = time()
self.lastDegrees = self.degrees
data = Imu()
data.header.stamp = rospy.Time.now()
data.header.frame_id = "imu_link"
data.header.seq = self.seq
data.orientation.w = wQ
data.orientation.x = xQ
data.orientation.y = yQ
data.orientation.z = zQ
data.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
data.linear_acceleration.x = 0.0
data.linear_acceleration.y = 0.0
data.linear_acceleration.z = 9.8
data.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
data.angular_velocity.x = 0.0
data.angular_velocity.y = 0.0
data.angular_velocity.z = wz
data.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
return data
def loop(self):
while not rospy.is_shutdown():
line = self.port.readline()
chunks = line.split(":")
if chunks[0] == "!QUAT":
readings = chunks[1].split(",")
if len(readings) == 10:
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'imu'
imu_msg.orientation.x = float(readings[0])
imu_msg.orientation.y = float(readings[1])
imu_msg.orientation.z = float(readings[2])
imu_msg.orientation.w = float(readings[3])
imu_msg.orientation_covariance = list(zeros((3,3)).flatten())
imu_msg.angular_velocity.x = float(readings[4])
imu_msg.angular_velocity.y = float(readings[5])
imu_msg.angular_velocity.z = float(readings[6])
imu_msg.angular_velocity_covariance = list(0.1*diagflat(ones((3,1))).flatten())
imu_msg.linear_acceleration.x = float(readings[7])
imu_msg.linear_acceleration.y = float(readings[8])
imu_msg.linear_acceleration.z = float(readings[9])
imu_msg.linear_acceleration_covariance = list(0.1*diagflat(ones((3,1))).flatten())
self.pub.publish(imu_msg)
quaternion = (imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w)
tf_br.sendTransform(translation = (0,0, 0), rotation = quaternion,time = rospy.Time.now(),child = 'imu',parent = 'world')
else:
rospy.logerr("Did not get a valid IMU packet, got %s", line)
else:
rospy.loginfo("Did not receive IMU data, instead got %s", line)
def main():
rospy.init_node('accelerometer', anonymous=False)
pub = rospy.Publisher("imu", Imu, queue_size=10)
rospy.loginfo('MMA8451 3DOF Accelerometer Publishing to IMU')
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_lsm6ds.LSM6DSOX(i2c)
imu_msg = Imu()
imu_msg.linear_acceleration_covariance = [
0, 0, 0,
0, 0, 0,
0, 0, 0
]
imu_msg.angular_velocity_covariance = [
0, 0, 0,
0, 0, 0,
0, 0, 0
]
while not rospy.is_shutdown():
x, y, z = sensor.acceleration
imu_msg.linear_acceleration.x = x
imu_msg.linear_acceleration.y = y
imu_msg.linear_acceleration.z = z
pub.publish(imu_msg)
rospy.sleep(1)
rospy.logwarn('MMA8451 Accelerometer Offline')
def publish_imu_data():
global imu, imu_publisher
imu_data = Imu()
imu_data.header.frame_id = "imu"
imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time)
# imu 3dmgx1 reports using the North-East-Down (NED) convention
# so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103
# see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros
q = imu.orientation
imu_data.orientation.w = q[0]
imu_data.orientation.x = q[2]
imu_data.orientation.y = q[1]
imu_data.orientation.z = -q[3]
imu_data.orientation_covariance = Config.get_orientation_covariance()
av = imu.angular_velocity
imu_data.angular_velocity.x = av[1]
imu_data.angular_velocity.y = av[0]
imu_data.angular_velocity.z = -av[2]
imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance(
)
la = imu.linear_acceleration
imu_data.linear_acceleration.x = la[1]
imu_data.linear_acceleration.y = la[0]
imu_data.linear_acceleration.z = -la[2]
imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance(
)
imu_publisher.publish(imu_data)
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 talker(aArgs):
pub = rospy.Publisher('ronin_imu', Imu, queue_size=10)
rospy.init_node('circle_test_source', anonymous=True)
rate = rospy.Rate(aArgs["sampling_freq"]) # Hz
step = 0
imu = Imu()
imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.orientation.x = 0.0
imu.orientation.y = 0.0
imu.orientation.z = 0.0
imu.orientation.w = 0.0
imu.angular_velocity.x = 0.0
imu.angular_velocity.y = 0.0
imu.linear_acceleration.z = 9.81
while not rospy.is_shutdown():
imu.header.seq = step
imu.header.stamp = rospy.Time.now()
angle = 2.0 * math.pi * aArgs["envelope_freq"] / aArgs[
"sampling_freq"] * step
omega = aArgs["omega_max"] * (1.0 - math.cos(angle)) / 2.0
imu.angular_velocity.z = omega
imu.linear_acceleration.x = math.pi * aArgs["envelope_freq"] * aArgs[
"omega_max"] * math.sin(angle) * aArgs["radius"]
imu.linear_acceleration.y = omega * omega * aArgs["radius"]
pub.publish(imu)
step = step + 1
rate.sleep()
def cmd_sync_callback(self, msg_pres_int, msg_imu_int):
msg_pres = FluidPressure();
msg_pres.header.stamp = rospy.Time.now()
msg_pres.header.frame_id = msg_pres_int.header.frame_id
msg_pres.fluid_pressure = (msg_pres_int.fluid_pressure * 1000) - self.atm_press
self.pub_pres.publish(msg_pres)
msg_imu = Imu();
msg_imu.header.stamp = rospy.Time.now()
msg_imu.header.frame_id = "/mur/imu_link"
qx = msg_imu_int.orientation.x
qy = msg_imu_int.orientation.y
qz = msg_imu_int.orientation.z
qw = msg_imu_int.orientation.w
euler_angles = euler_from_quaternion(np.array([qx,qy,qz,qw]))
r = euler_angles[1]
p = -euler_angles[0]
y = euler_angles[2]
q = quaternion_from_euler(r,p,y)
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
msg_imu.orientation.w = q[3]
msg_imu.orientation_covariance = np.array([0,0,0,0,0,0,0,0,0])
msg_imu.angular_velocity.x = -msg_imu_int.angular_velocity.x
msg_imu.angular_velocity.y = msg_imu_int.angular_velocity.y
msg_imu.angular_velocity.z = msg_imu_int.angular_velocity.z
msg_imu.angular_velocity_covariance = np.array([1.2184E-7,0.0,0.0,0.0,1.2184E-7,0.0,0.0,0.0,1.2184E-7])
msg_imu.linear_acceleration.x = msg_imu_int.linear_acceleration.x # The same configuration in the PIXHAWK
msg_imu.linear_acceleration.y = msg_imu_int.linear_acceleration.y
msg_imu.linear_acceleration.z = msg_imu_int.linear_acceleration.z
msg_imu.linear_acceleration_covariance = np.array([9.0E-8,0.0,0.0,0.0,9.0E-8,0.0,0.0,0.0,9.0E-8])
self.pub_imu.publish(msg_imu)
def grv_data_handler(self, timestamp, grv_w, grv_x, grv_y, grv_z):
# publish Imu
gyr_scale = (500.0 / 32768.0) / 180.0 * math.pi # 500dps FSR
acc_scale = (4.0 / 32768.0) * 9.81 # 4g FSR
msg = Imu()
msg.header.stamp = rospy.get_rostime()
#msg.header.frame_id = 'base_link' # for test
msg.angular_velocity.x = self.gyr_raw_x * gyr_scale
msg.angular_velocity.y = self.gyr_raw_y * gyr_scale
msg.angular_velocity.z = self.gyr_raw_z * gyr_scale
msg.angular_velocity_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
msg.linear_acceleration.x = self.acc_raw_x * acc_scale
msg.linear_acceleration.y = self.acc_raw_y * acc_scale
msg.linear_acceleration.z = self.acc_raw_z * acc_scale
msg.linear_acceleration_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
msg.orientation.w = float(grv_w) / 2**30
msg.orientation.x = float(grv_x) / 2**30
msg.orientation.y = float(grv_y) / 2**30
msg.orientation.z = float(grv_z) / 2**30
msg.orientation_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
self.send(self.pub_imu, msg)
def callback_imu(data):
# rospy.loginfo(data.poses[])
imu_data = Imu()
imu_data.header.frame_id = "base_link"
# poses[0].position-> ax, ay, az (linear acc)
imu_data.linear_acceleration.x = data.poses[0].position.x
imu_data.linear_acceleration.y = data.poses[0].position.y
imu_data.linear_acceleration.z = data.poses[0].position.z
#poses[0].orientation orientation quaternion
imu_data.orientation.x = data.poses[0].orientation.x
imu_data.orientation.y = data.poses[0].orientation.y
imu_data.orientation.z = data.poses[0].orientation.z
imu_data.orientation.w = data.poses[0].orientation.w
#poses[1].position gx gy gz
imu_data.angular_velocity.x = data.poses[1].position.x
imu_data.angular_velocity.y = data.poses[1].position.y
imu_data.angular_velocity.z = data.poses[1].position.z
imu_data.angular_velocity_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3]
imu_data.linear_acceleration_covariance = [
1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3
]
imu_data.orientation_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3]
imu_data.header.stamp = rospy.Time.now()
pub.publish(imu_data)
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 ros_imu(info):
for index, item in enumerate(info):
info[index] = float(item)
imuMsg = Imu()
imuMsg.orientation_covariance = [0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025]
imuMsg.angular_velocity_covariance = [0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02]
imuMsg.linear_acceleration_covariance = [
0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04
]
Accel_magnitude = math.sprt(info[0] * info[0] + info[1] * info[1] +
info[2] * info[2])
Accel_magnitude = float(Accel_magnitude / GRAVITY)
pitch = -math.atan2(info[1],
math.sqrt(info[2] * info[2] + info[3] * info[3]))
roll = -math.atan2(info[2], info[3])
xh = info[7] * math.cos(pitch) + info[8] * math.sin(roll) * math.sin(
pitch) + info[9] * math.sin(pitch) * math.cos(roll)
yh = info[8] * math.cos(roll) + info[9] * math.sin(roll)
yaw = math.atan2(-yh, xh)
seq = 0
#print roll
q = quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'base_imu_link'
imuMsg.header.seq = seq
seq = seq + 1
imu_pose(imuMsg)
gpspub.publish(imuMsg)
def publish_imu(self):
imu_msg = Imu()
imu_msg.header.stamp = self.time
imu_msg.header.frame_id = 'imu_odom'
#quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz')
a = self.kalman_state[3,0]
b = self.kalman_state[4,0]
c = self.kalman_state[5,0]
d = self.kalman_state[6,0]
q = math.sqrt(math.pow(a,2)+math.pow(b,2)+math.pow(c,2)+math.pow(d,2))
#angles = tf_math.euler_from_quaternion(self.kalman_state[3:7,0])
imu_msg.orientation.x = a/q#angles[0]
imu_msg.orientation.y = b/q
imu_msg.orientation.z = c/q
imu_msg.orientation.w = d/q
imu_msg.orientation_covariance = list(self.kalman_covariance[3:6,3:6].flatten())
imu_msg.angular_velocity.x = self.kalman_state[0,0]
imu_msg.angular_velocity.y = self.kalman_state[1,0]
imu_msg.angular_velocity.z = self.kalman_state[2,0]
imu_msg.angular_velocity_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
imu_msg.linear_acceleration.x = self.kalman_state[10,0]
imu_msg.linear_acceleration.y = self.kalman_state[11,0]
imu_msg.linear_acceleration.z = self.kalman_state[12,0]
imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[10:13,10:13].flatten())
self.pub.publish(imu_msg)
def publish(self):
imu_msg = Imu()
imu_msg.header.stamp = rospy.get_rostime()
imu_msg.header.frame_id = "imu_link"
imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance
imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
#imu_msg.orientation_covariance = self.orientation_covariance
imu_msg.linear_acceleration.x = self.linear_acceleration[0] - self.la_offset[0]
imu_msg.linear_acceleration.y = self.linear_acceleration[1] - self.la_offset[1]
imu_msg.linear_acceleration.z = self.linear_acceleration[2] - self.la_offset[2]
imu_msg.angular_velocity.x = (self.angular_velocity[0] - self.av_offset[0]) * (np.pi / 180.0)
imu_msg.angular_velocity.y = (self.angular_velocity[1] - self.av_offset[1]) * (np.pi / 180.0)
imu_msg.angular_velocity.z = (self.angular_velocity[2] - self.av_offset[2]) * (np.pi / 180.0)
#if abs(self.angular_velocity[2] - self.av_offset[2]) < 3:
# imu_msg.linear_acceleration.x = 0.0
# imu_msg.linear_acceleration.y = 0.0
# imu_msg.linear_acceleration.z = 0.0
# imu_msg.angular_velocity.x = 0.0
# imu_msg.angular_velocity.y = 0.0
# imu_msg.angular_velocity.z = 0.0
self.imu_pub.publish(imu_msg)
def _get_sensor_msg_imu(self, timestamp):
# 1 g-unit is equal to 9.80665 meter/square second. # Linear Acceleration
# 1° × pi/180 = 0.01745rad # Angular velocity # Gyro
gunit_to_mps_squ = 9.80665
msg = Imu()
msg.header.stamp = timestamp
msg.header.frame_id = IMU_FRAME
gyr = self.sense.get_gyroscope_raw()
acc = self.sense.get_accelerometer_raw()
msg.orientation.x = 0.0
msg.orientation.y = 0.0
msg.orientation.z = 0.0
msg.orientation.w = 0.0
msg.orientation_covariance = [-1, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0]
#msg.angular_velocity.x = gyr['x'] * np.pi
msg.angular_velocity.x = round( gyr['x'] * ( np.pi / 180 ), 8 ) * (-1) # Inverted x axis
msg.angular_velocity.y = round( gyr['y'] * ( np.pi / 180 ), 8 )
msg.angular_velocity.z = round( gyr['z'] * ( np.pi / 180 ), 8 )
msg.angular_velocity_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg.linear_acceleration.x = round((acc['x'] * gunit_to_mps_squ), 8 ) * (-1) # Inverted x axis
msg.linear_acceleration.y = round((acc['y'] * gunit_to_mps_squ), 8 )
msg.linear_acceleration.z = round((acc['z'] * gunit_to_mps_squ), 8 )
#msg.linear_acceleration.x = np.radians( acc['x'] )
#msg.linear_acceleration.y = np.radians( acc['y'] )
#msg.linear_acceleration.z = np.radians( acc['z'] )
msg.linear_acceleration_covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
return msg
def get_rotation(msg):
global roll, pitch, yaw
orientation_q = msg.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
print 'yaw', yaw, 'roll', roll, 'pitch', pitch
current_time = rospy.Time.now()
imu = Imu()
imu.header.stamp = current_time
imu.header.frame_id = "base_link"
imu.orientation.x = -msg.orientation.y #pitch
imu.orientation.y = msg.orientation.x #roll
imu.orientation.z = msg.orientation.z #yaw
imu.orientation.w = msg.orientation.w
imu.orientation_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
imu.angular_velocity.x = -msg.angular_velocity.y
imu.angular_velocity.y = msg.angular_velocity.x
imu.angular_velocity.z = msg.angular_velocity.z
imu.angular_velocity_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
imu.linear_acceleration.x = -msg.linear_acceleration.y
imu.linear_acceleration.y = msg.linear_acceleration.x
imu.linear_acceleration.z = msg.linear_acceleration.z
imu.linear_acceleration_covariance = [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01]
euler_pub.publish(imu)
def update_sensors(self):
# Accelerometer
accel = self._driver.get_accelerometer()
accel_msg = Imu()
accel_msg.header.stamp = rospy.Time.now()
accel_msg.header.frame_id = self._name + "/base_link"
accel_msg.linear_acceleration.x = (
accel[1] - 2048.0
) / 800.0 * 9.81 # 1 g = about 800, then transforms in m/s^2.
accel_msg.linear_acceleration.y = (accel[0] - 2048.0) / 800.0 * 9.81
accel_msg.linear_acceleration.z = (accel[2] - 2048.0) / 800.0 * 9.81
accel_msg.linear_acceleration_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
#print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2])
#print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81)
accel_msg.angular_velocity.x = 0
accel_msg.angular_velocity.y = 0
accel_msg.angular_velocity.z = 0
accel_msg.angular_velocity_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
q = tf.transformations.quaternion_from_euler(0, 0, 0)
accel_msg.orientation = Quaternion(*q)
accel_msg.orientation_covariance = [
0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01
]
self.accel_publisher.publish(accel_msg)
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 publish_imu_data():
global imu, imu_publisher
imu_data = Imu()
imu_data.header.frame_id = "imu"
imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time)
# imu 3dmgx1 reports using the North-East-Down (NED) convention
# so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103
# see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros
q = imu.orientation
imu_data.orientation.w = q[0]
imu_data.orientation.x = q[2]
imu_data.orientation.y = q[1]
imu_data.orientation.z = -q[3]
imu_data.orientation_covariance = Config.get_orientation_covariance()
av = imu.angular_velocity
imu_data.angular_velocity.x = av[1]
imu_data.angular_velocity.y = av[0]
imu_data.angular_velocity.z = -av[2]
imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance()
la = imu.linear_acceleration
imu_data.linear_acceleration.x = la[1]
imu_data.linear_acceleration.y = la[0]
imu_data.linear_acceleration.z = - la[2]
imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance()
imu_publisher.publish(imu_data)
def main():
rospy.init_node('SensorLocalisationReciever', anonymous=True)
print("Running SensorLocalisationReciever")
# ser = serial.Serial(rospy.get_param('~sensorserialport','/dev/ttyACM0'))
imu_data = Imu()
gps_data = NavSatFix()
imu_data.header.frame_id = 'base_link'
gps_data.header.frame_id = 'map'
rate = rospy.Rate(rospy.get_param("~rate", 100))
imu_pub = rospy.Publisher('/imu/data', Imu, queue_size=10)
gps_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
while not rospy.is_shutdown():
# print(x,y)
# data = ser.readline()
# if data[0]=='#':
# data = data[1:]
# linear,angular = data.split('/')[0],data.split('/')[1]
linear = [float(x) for x in linear.split(' ')]
angular = [float(x) for x in angular.split(' ')]
imu_data.orientation.x = 1 ##angular[3]
imu_data.orientation.y = 1 ## angular[4]
imu_data.orientation.z = 1 ##angular[5]
imu_data.linear_acceleration.x = 1 # linear[6]
imu_data.linear_acceleration.y = 1 # linear[7]
imu_data.linear_acceleration.z = 1 #linear[8]
imu_data.angular_velocity_covariance = [
1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0
]
imu_data.linear_acceleration_covariance = [
1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0
]
gps_data.latitude = 1 # linear[0]
gps_data.longitude = 1 #linear[1]
gps_data.position_covariance = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
gps_data.position_covariance_type = 3
gps_data.header.stamp = imu_data.header.stamp = rospy.Time.now()
imu_pub.publish(imu_data)
gps_pub.publish(gps_data)
print(linear)
# print(angular)
rate.sleep()
def readIMU(self):
imuMsg = Imu()
# Orientation covariance estimation:
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
# Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
# Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
# cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
# i.e. variance in yaw: 0.0025
# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
# static roll/pitch error of 0.8%, owing to gravity orientation sensing
# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
# so set all covariances the same.
imuMsg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
# Angular velocity covariance estimation:
# Observed gyro noise: 4 counts => 0.28 degrees/sec
# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec
# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02
imuMsg.angular_velocity_covariance = [
0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02
]
# linear acceleration covariance estimation:
# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2
# nonliniarity spec: 0.5% of full scale => 0.2m/s^2
# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
imuMsg.linear_acceleration_covariance = [
0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04
]
imuMsg.header.frame_id = 'imu_link'
imuMsg.header.stamp = rospy.Time.now()
accel_factor = 9.806 / 256.0
#rospy.loginfo('Reading IMU data')
self.ser.write(b'?IMU\n')
countout = 0
while True:
data = self.ser.read_until('\n', None)
#rospy.loginfo(data)
if data.startswith('IMU='):
imu = data.strip().replace('IMU=', '').split(' ')
imuMsg.orientation.x = float(imu[0])
imuMsg.orientation.y = float(imu[1])
imuMsg.orientation.z = float(imu[2])
imuMsg.orientation.w = float(imu[3])
imuMsg.linear_acceleration.x = -float(imu[5]) * accel_factor
imuMsg.linear_acceleration.y = float(imu[5]) * accel_factor
imuMsg.linear_acceleration.z = float(imu[6]) * accel_factor
imuMsg.angular_velocity.x = float(imu[7])
imuMsg.angular_velocity.y = -float(imu[8])
imuMsg.angular_velocity.z = -float(imu[9])
self.imu_pub.publish(imuMsg)
break
countout = countout + 1
if countout > 3:
break
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 _create_imu_msg(self):
# Check if data is available
if 'ATTITUDE' not in self.get_data():
raise Exception('no ATTITUDE data')
#TODO: move all msgs creating to msg
msg = Imu()
self._create_header(msg)
#http://mavlink.org/messages/common#SCALED_IMU
imu_data = None
for i in ['', '2', '3']:
try:
imu_data = self.get_data()['SCALED_IMU{}'.format(i)]
break
except Exception as e:
pass
if imu_data is None:
raise Exception('no SCALED_IMUX data')
acc_data = [imu_data['{}acc'.format(i)] for i in ['x', 'y', 'z']]
gyr_data = [imu_data['{}gyro'.format(i)] for i in ['x', 'y', 'z']]
mag_data = [imu_data['{}mag'.format(i)] for i in ['x', 'y', 'z']]
#http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
msg.linear_acceleration.x = acc_data[0] / 100
msg.linear_acceleration.y = acc_data[1] / 100
msg.linear_acceleration.z = acc_data[2] / 100
msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
msg.angular_velocity.x = gyr_data[0] / 1000
msg.angular_velocity.y = gyr_data[1] / 1000
msg.angular_velocity.z = gyr_data[2] / 1000
msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
#http://mavlink.org/messages/common#ATTITUDE
attitude_data = self.get_data()['ATTITUDE']
orientation = [attitude_data[i] for i in ['roll', 'pitch', 'yaw']]
#https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_Angles_to_Quaternion_Conversion
cy = math.cos(orientation[2] * 0.5)
sy = math.sin(orientation[2] * 0.5)
cr = math.cos(orientation[0] * 0.5)
sr = math.sin(orientation[0] * 0.5)
cp = math.cos(orientation[1] * 0.5)
sp = math.sin(orientation[1] * 0.5)
msg.orientation.w = cy * cr * cp + sy * sr * sp
msg.orientation.x = cy * sr * cp - sy * cr * sp
msg.orientation.y = cy * cr * sp + sy * sr * cp
msg.orientation.z = sy * cr * cp - cy * sr * sp
msg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
self.pub.set_data('/imu/data', 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._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 update_imu(self):
"""
Reads all characters in the buffer until finding \r\n
Messages should have the following format: "Q: w x y z | A: x y z | G: x y z"
:return:
"""
# Create new message
try:
imuMsg = Imu()
# Set the sensor covariances
imuMsg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
imuMsg.angular_velocity_covariance = [-1., 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.linear_acceleration_covariance = [
-1., 0, 0, 0, 0, 0, 0, 0, 0
]
imu_data = self.read_data()
if len(imu_data) == 0:
self.log("IMU is not answering", 2)
return
q_data = transformations.quaternion_from_euler(
imu_data[2], imu_data[1], imu_data[0])
q1 = Quaternion()
q1.x = float(q_data[0])
q1.y = float(q_data[1])
q1.z = float(q_data[2])
q1.w = float(q_data[3])
q_off = self.imu_offset
new_q = transformations.quaternion_multiply(
[q1.x, q1.y, q1.z, q1.w], [q_off.x, q_off.y, q_off.z, q_off.w])
imuMsg.orientation.x = new_q[0]
imuMsg.orientation.y = new_q[1]
imuMsg.orientation.z = new_q[2]
imuMsg.orientation.w = new_q[3]
# Handle message header
imuMsg.header.frame_id = "base_link_imu"
imuMsg.header.stamp = rospy.Time.now() + rospy.Duration(nsecs=5000)
self.imu_reading = imuMsg
except SerialException as serial_exc:
self.log(
"SerialException while reading from IMU: {}".format(
serial_exc), 3)
self.calibration = True
except ValueError as val_err:
self.log("Value error from IMU data - {}".format(val_err), 5)
self.val_exc = self.val_exc + 1
except Exception as imu_exc:
self.log(imu_exc, 3)
raise imu_exc
def GetImuFromLine(line): (ts, ax, ay, az, gx, gy, gz) = [t(s) for t,s in zip((int,int, int, int, int, int, int),line.split())] imu = Imu() ts = float(ts)/1000000.0 ImuStamp = rospy.rostime.Time.from_sec(ts) imu.angular_velocity = create_vector3(gyro_raw_to_rads(gx), gyro_raw_to_rads(gy), gyro_raw_to_rads(gz)) imu.angular_velocity_covariance = create_diag_mat(gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0)); imu.linear_acceleration = create_vector3(acc_raw_to_ms(ax), acc_raw_to_ms(ay), acc_raw_to_ms(az)) imu.linear_acceleration_covariance = create_diag_mat(acc_raw_to_ms(1.0), acc_raw_to_ms(1.0), acc_raw_to_ms(1.0)) return (ts, ImuStamp, imu)
def main():
rospy.init_node("imu_node")
pub = rospy.Publisher('imu/data', Imu, queue_size=10)
global yaw
global rover_accx
yaw_old = yaw
vth = 0
imuMsg = Imu()
imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
current_time = rospy.Time.now()
last_time = rospy.Time.now()
print("Sleeping 1 second")
rospy.sleep(1)
# while(1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Will be obtained from sensor
current_time = rospy.Time.now()
dt = (current_time - last_time).to_sec()
if (yaw > 0):
dyaw = yaw - yaw_old
vth = dyaw / dt
if (yaw < 0):
dyaw = yaw - yaw_old
vth = dyaw / dt
# Acceloremeter
imuMsg.linear_acceleration.x = rover_accx
imuMsg.linear_acceleration.y = 0
imuMsg.linear_acceleration.z = 0
# Gyro
imuMsg.angular_velocity.x = 0
imuMsg.angular_velocity.y = 0
imuMsg.angular_velocity.z = vth
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
imuMsg.orientation.x = q[0] #magnetometer
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'base_link'
rospy.loginfo(imuMsg)
pub.publish(imuMsg)
yaw_old = yaw
last_time = rospy.Time.now()
rospy.Subscriber('/rover_serial_imu', String, callback_sensor)
rate.sleep()
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 call_imu(self, msg_imu_int):
# IMU
msg_imu = Imu();
msg_imu.header.stamp = rospy.Time.now()
msg_imu.header.frame_id = "world"
r,p,y = euler_from_quaternion([msg_imu_int.orientation.x,msg_imu_int.orientation.y,msg_imu_int.orientation.z,msg_imu_int.orientation.w])
self.roll = -p
self.pitch = r
self.yaw = y
q_new = quaternion_from_euler(self.roll, self.pitch, self.yaw)
#rospy.loginfo(q_new)
rospy.loginfo(euler_from_quaternion(q_new))
msg_imu.orientation.x = q_new[0]
msg_imu.orientation.y = q_new[1]
msg_imu.orientation.z = q_new[2]
msg_imu.orientation.w = q_new[3]
msg_imu.orientation_covariance = msg_imu_int.orientation_covariance
msg_imu.angular_velocity.x = -msg_imu_int.angular_velocity.x
msg_imu.angular_velocity.y = -msg_imu_int.angular_velocity.y
msg_imu.angular_velocity.z = msg_imu_int.angular_velocity.z
msg_imu.angular_velocity_covariance = msg_imu_int.angular_velocity_covariance
msg_imu.linear_acceleration.x = msg_imu_int.linear_acceleration.y # The same configuration in the PIXHAWK
msg_imu.linear_acceleration.y = -msg_imu_int.linear_acceleration.x
msg_imu.linear_acceleration.z = msg_imu_int.linear_acceleration.z
msg_imu.linear_acceleration_covariance = msg_imu_int.linear_acceleration_covariance
self.pub_imu.publish(msg_imu)
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.frame_id = "world"
t.header.stamp = rospy.Time.now()
t.child_frame_id = "odom"
t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = self.z
t.transform.rotation.x = q_new[0]
t.transform.rotation.y = q_new[1]
t.transform.rotation.z = q_new[2]
t.transform.rotation.w = q_new[3]
br.sendTransform(t)
# ORIENTATION
msg_ori = PointStamped()
msg_ori.header.stamp = rospy.Time.now()
msg_ori.header.frame_id = "world"
msg_ori.point.x = self.pitch
msg_ori.point.y = self.roll
msg_ori.point.z = self.yaw
self.pub_ori.publish(msg_ori)
# ANGULAR VELOCITY
msg_vel = PointStamped()
msg_vel.header.stamp = rospy.Time.now()
msg_vel.header.frame_id = "world"
msg_vel.point.x = msg_imu.angular_velocity.x
msg_vel.point.y = msg_imu.angular_velocity.y
msg_vel.point.z = msg_imu.angular_velocity.z
self.pub_omega.publish(msg_vel)
def Vn100Pub():
pub = rospy.Publisher('IMUData', Imu, queue_size=1)
pub2 = rospy.Publisher('IMUMag', MagneticField, queue_size=1)
# Initialize the node and name it.
rospy.init_node('IMUpub')
vn = imuthread3.Imuthread(port=rospy.get_param("imu_port"), pause=0.0)
vn.start()
vn.set_data_freq50()
vn.set_qmr_mode()
#vn.set_data_freq10() #to see if this fixes my gps drop out problem
rospy.sleep(3)
msg = Imu()
msg2 = MagneticField()
count = 0
while not rospy.is_shutdown():
if len(vn.lastreadings)>0:
if vn.lastreadings[0] =='VNQMR':
msg.header.seq = count
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu'
msg.orientation.x = float(vn.lastreadings[1])
msg.orientation.y = float(vn.lastreadings[2])
msg.orientation.z = float(vn.lastreadings[3])
msg.orientation.w = float(vn.lastreadings[4])
msg.orientation_covariance = [0,0,0,0,0,0,0,0,0]
msg.angular_velocity.x = float(vn.lastreadings[8])
msg.angular_velocity.y = float(vn.lastreadings[9])
msg.angular_velocity.z = float(vn.lastreadings[10])
msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0]
msg.linear_acceleration.x = float(vn.lastreadings[11])
msg.linear_acceleration.y = float(vn.lastreadings[12])
msg.linear_acceleration.z = float(vn.lastreadings[13])
msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0]
msg2.header.seq = count
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = 'imu'
msg2.magnetic_field.x = float(vn.lastreadings[5])
msg2.magnetic_field.x = float(vn.lastreadings[6])
msg2.magnetic_field.x = float(vn.lastreadings[7])
msg2.magnetic_field_covariance = [0,0,0,0,0,0,0,0,0]
#rospy.loginfo("vn100_pub " + str(msg.header.stamp) + " " + str(msg.orientation.x) + " "+ str(msg.orientation.y) + " "+ str(msg.orientation.z) + " "+ str(msg.orientation.w) + " ")
current_pose_euler = tf.transformations.euler_from_quaternion([
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
])
pub.publish(msg)
pub2.publish(msg2)
count += 1
#rospy.sleep(1.0/100.0)
vn.kill = True
def talker():
global oldTime
pubString = rospy.Publisher('sensor/string', String, queue_size=1000)
pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000)
rospy.init_node('sensornimureader', anonymous=True)
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
# sample data
currentTime = rospy.Time.now()
timeCheck = currentTime.to_sec() - oldTime.to_sec()
print timeCheck
if (timeCheck) > 2.0:
data = 'Z1.341725,103.965008,1.5100,0.0000'
oldTime = currentTime;
else: data = 'Y0.0000,0.0730,255.4516,1.5100,0.0000,0.0000,0.000,0.000,0.000'
# data = ser.readline()
pubString.publish(data)
if data[0] == 'Y':
data = data.replace("Y","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 9:
try:
##data_list structure: accel, magni, gyro
float_list = [float(i) for i in data_list]
imuData = Imu()
imuData.header.frame_id = "base_link"
imuData.header.stamp = rospy.Time.now()
##data form in yaw, pitch, roll
quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
imuData.orientation.x = quat[0]
imuData.orientation.y = quat[1]
imuData.orientation.z = quat[2]
imuData.orientation.w = quat[3]
imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
imuData.linear_acceleration.x = float_list[0]*accel_scale
imuData.linear_acceleration.y = -float_list[1]*accel_scale
imuData.linear_acceleration.z = -float_list[2]*accel_scale
imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6]
imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7]
imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3]
pubIMU.publish(imuData)
log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
% (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
float_list[3], float_list[4], float_list[5],
imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time())
except: log = "IMU Data Error! Data : %s" % data
else: log = "Data Error! Data : %s" % data
rospy.loginfo(log)
rate.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] == '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 talker():
pub = rospy.Publisher('IMUBoat', Imu, queue_size=1000)
rospy.init_node('callibIMU', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
data = ser.readline()
# print data
# print len(data)
if data[0] == 'Y' and len(data) >= 70 and len(data) <= 85:
data = data.replace("Y","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 9:
try:
##data_list structure: accel, magni, gyro
float_list = [float(i) for i in data_list]
imuData = Imu()
imuData.header.frame_id = "imu"
imuData.header.stamp = rospy.Time.now()
quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
imuData.orientation.x = quat[0]
imuData.orientation.y = quat[1]
imuData.orientation.z = quat[2]
imuData.orientation.w = quat[3]
imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
imuData.linear_acceleration.x = float_list[0]*accel_scale
imuData.linear_acceleration.y = -float_list[1]*accel_scale
imuData.linear_acceleration.z = -float_list[2]*accel_scale
imuData.orientation_covariance = []
imuData.angular_velocity_covariance = []
imuData.linear_acceleration_covariance = []
pub.publish(imuData)
# log = "Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
# % (float_list[0], float_list[1], float_list[2], float_list[3],
# float_list[4], float_list[5], float_list[6], float_list[7],
# float_list[8], rospy.get_time())
print "%f %f %f %f %f %f %f %f %f" % (
imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
float_list[3], float_list[4], float_list[5],
imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z)
except: continue #log = "Data Error! Data : %s" % data
else: continue #log = "Data Error! Data : %s" % data
#rospy.loginfo(log)
else:
continue
#log = "Non Data Serial Message: %s" % data
#rospy.loginfo(log)
rate.sleep()
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 msg_template(self):
msg = Imu()
msg.header.frame_id = "robocape"
msg.linear_acceleration_covariance = (0.1, 0.0, 0.0,
0.0, 0.1, 0.0,
0.0, 0.0, 0.1)
msg.angular_velocity_covariance = (1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0)
msg.orientation_covariance = (10.0, 0.0, 0.0,
0.0, 10.0, 0.0,
0.0, 0.0, 10.0)
return msg
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 publish_imu(self):
imu_msg = Imu()
imu_msg.header.stamp = self.time
imu_msg.header.frame_id = 'imu'
imu_msg.orientation = Quaternion(*SF9DOF_UKF.recover_quat(self.kalman_state))
imu_msg.orientation_covariance = \
list(self.kalman_covariance[0:3,0:3].flatten())
imu_msg.angular_velocity.x = self.kalman_state[3,0]
imu_msg.angular_velocity.y = self.kalman_state[4,0]
imu_msg.angular_velocity.z = self.kalman_state[5,0]
imu_msg.angular_velocity_covariance = \
list(self.kalman_covariance[3:6,3:6].flatten())
imu_msg.linear_acceleration.x = self.kalman_state[6,0]
imu_msg.linear_acceleration.y = self.kalman_state[7,0]
imu_msg.linear_acceleration.z = self.kalman_state[8,0]
imu_msg.linear_acceleration_covariance = \
list(self.kalman_covariance[6:9,6:9].flatten())
self.pub.publish(imu_msg)
def publish_imu(self):
imu_msg = Imu()
imu_msg.header.stamp = self.time
imu_msg.header.frame_id = 'imu_odom'
quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz')
imu_msg.orientation.x = quat[0]
imu_msg.orientation.y = quat[1]
imu_msg.orientation.z = quat[2]
imu_msg.orientation.w = quat[3]
imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
imu_msg.angular_velocity.x = self.kalman_state[3,0]
imu_msg.angular_velocity.y = self.kalman_state[4,0]
imu_msg.angular_velocity.z = self.kalman_state[5,0]
imu_msg.angular_velocity_covariance = list(self.kalman_covariance[3:6,3:6].flatten())
imu_msg.linear_acceleration.x = self.kalman_state[11,0]
imu_msg.linear_acceleration.y = self.kalman_state[12,0]
imu_msg.linear_acceleration.z = self.kalman_state[13,0]
imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[11:14,11:14].flatten())#[.01, 0, 0, 0, .01, 0, 0, 0, .01]
self.pub.publish(imu_msg)
def talker():
#Create publisher ('Topic name', msg type)
pub = rospy.Publisher('IMU', Imu)
#Tells rospy name of the node to allow communication to ROS master
rospy.init_node('IMUtalker')
while not rospy.is_shutdown():
#Grab relevant information from parse()
try:
(accel,gyro,magne) = parse()
#If data is bad, uses presvious data
except: continue
#Define IMUmsg to be of the Imu msg type
IMUmsg = Imu()
#Set header time stamp
IMUmsg.header.stamp = rospy.Time.now()
#Set orientation parameters
IMUmsg.orientation = Quaternion()
IMUmsg.orientation.x = magne[0]
IMUmsg.orientation.y = magne[1]
IMUmsg.orientation.z = magne[2]
IMUmsg.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
#Set angular velocity parameters
IMUmsg.angular_velocity = Vector3()
IMUmsg.angular_velocity.x = gyro[0]
IMUmsg.angular_velocity.y = gyro[1]
IMUmsg.angular_velocity.z = gyro[2]
IMUmsg.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
#Set linear acceleration parameters
IMUmsg.linear_acceleration = Vector3()
IMUmsg.linear_acceleration.x = accel[0]
IMUmsg.linear_acceleration.y = accel[1]
IMUmsg.linear_acceleration.z = accel[2]
IMUmsg.linear_acceleration_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
#Publish the data
pub.publish(IMUmsg)
def run_arduino(self):
rospy.init_node('arduino_driver', anonymous=False)
print "Arduino Driver is Running!"
while not rospy.is_shutdown():
message=self.ser_obj.readline()
result=message.strip().split(',')
if len(result)==7:
imu_message=Imu()
imu_message.header.stamp=rospy.Time.now()
imu_message.header.frame_id="gyro_link"
imu_message.orientation.x=0.0
imu_message.orientation.y=0.0
imu_message.orientation.z=0.0
imu_message.orientation.w=1.0
imu_message.orientation_covariance=[-1, 0, 0, 0, -1, 0, 0, 0, -1]
imu_message.linear_acceleration.x=float(result[0])
imu_message.linear_acceleration.y=float(result[1])
imu_message.linear_acceleration.z=float(result[2])
imu_message.linear_acceleration_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
imu_message.angular_velocity.x=float(result[3])
imu_message.angular_velocity.y=float(result[4])
imu_message.angular_velocity.z=float(result[5])
imu_message.angular_velocity_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
# accel=result[0:3]
# gyro=result[3:6]
# batt=result[6]
#res=[accel, gyro, batt]
# self.imu_pub.publish(imu_message)
self.imu_pub.publish(imu_message)
self.batt_pub.publish(float(result[6]))
def publish_imu(self):
imu_msg = Imu()
imu_msg.header.stamp = self.time
imu_msg.header.frame_id = 'imu_odom'
imu_msg.orientation.x = self.kalman_state[0,0]
imu_msg.orientation.y = self.kalman_state[1,0]
imu_msg.orientation.z = self.kalman_state[2,0]
imu_msg.orientation.w = self.kalman_state[3,0]
imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
imu_msg.angular_velocity.x = self.kalman_state[4,0]
imu_msg.angular_velocity.y = self.kalman_state[5,0]
imu_msg.angular_velocity.z = self.kalman_state[6,0]
# Check this covariance
imu_msg.angular_velocity_covariance = list(self.kalman_covariance[4:7,4:7].flatten())
imu_msg.linear_acceleration.x = self.measurement[0,0]
imu_msg.linear_acceleration.y = self.measurement[1,0]
imu_msg.linear_acceleration.z = self.measurement[2,0]
acc_cov = SF9DOF_UKF.measurement_noise(self.measurement, 1.0)[0:3,0:3]
imu_msg.linear_acceleration_covariance = list(acc_cov.flatten())
self.pub.publish(imu_msg)
def update_sensors(self): # Accelerometer accel = self._driver.get_accelerometer() accel_msg = Imu() accel_msg.header.stamp = rospy.Time.now() accel_msg.header.frame_id = self._name+"/base_link" accel_msg.linear_acceleration.x = (accel[1]-2048.0)/800.0*9.81 # 1 g = about 800, then transforms in m/s^2. accel_msg.linear_acceleration.y = (accel[0]-2048.0)/800.0*9.81 accel_msg.linear_acceleration.z = (accel[2]-2048.0)/800.0*9.81 accel_msg.linear_acceleration_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] #print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2]) #print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81) accel_msg.angular_velocity.x = 0 accel_msg.angular_velocity.y = 0 accel_msg.angular_velocity.z = 0 accel_msg.angular_velocity_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] q = tf.transformations.quaternion_from_euler(0, 0, 0) accel_msg.orientation = Quaternion(*q) accel_msg.orientation_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] self.accel_publisher.publish(accel_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] == '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 msg_template(self):
msg_imu = Imu();
msg_mag = MagneticField();
msg_imu.header.frame_id = "robocape";
msg_mag.header.frame_id = "robocape_mag";
msg_imu.linear_acceleration_covariance = (0.1, 0.0, 0.0,
0.0, 0.1, 0.0,
0.0, 0.0, 0.1);
msg_imu.angular_velocity_covariance = (1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
msg_imu.orientation_covariance = (-1.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0);
msg_mag.magnetic_field_covariance = (10.0, 0.0, 0.0,
0.0, 10.0, 0.0,
0.0, 0.0, 10.0);
return (msg_imu, msg_mag);
def publish_imu():
global buffer,gyro_cal,previousTime,yaw,angular_vel
currentTime=rospy.Time.now()
dt=currentTime-previousTime
dt=dt.to_sec()
previousTime=currentTime
if rVel==0 and lVel==0:
buffer.append(gyroRaw)
if len(buffer) > buffer_length:
buffer.pop(0)
gyro_cal=numpy.mean(buffer)
angular_vel=0
else:
angular_vel=-(((gyroRaw-gyro_cal)/gyro_cal)*300*gyroScale)*math.pi/180;
if math.fabs(angular_vel) < 0:
angular_vel=0
yaw+=(angular_vel*dt)
rospy.loginfo("Yaw %s" % yaw)
rospy.loginfo("Yaw Vel %s" % angular_vel)
imu=Imu()
imu.header.stamp = currentTime
imu.header.frame_id = 'gyro_link'
imu.orientation.x=0
imu.orientation.y=0
imu.orientation.z=math.sin(yaw/2)
imu.orientation.w=math.cos(yaw/2)
imu.orientation_covariance=[1e6,0,0,0,1e6,0,0,0,1e-10]
imu.angular_velocity.x=0
imu.angular_velocity.y=0
imu.angular_velocity.z=angular_vel
imu.angular_velocity_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-10]
imu.linear_acceleration_covariance=[-1,0,0,0,0,0,0,0,0]
imuPub.publish(imu)
def talker(message):
corrected = Imu()
corrected.header.seq = message.header.seq
corrected.header.stamp = rospy.Time.now()
corrected.header.frame_id = message.header.frame_id
# Transpose
corrected.linear_acceleration.x = message.linear_acceleration.y
corrected.linear_acceleration.y = -message.linear_acceleration.x
corrected.linear_acceleration.z = -message.linear_acceleration.z
corrected.angular_velocity.x = message.angular_velocity.x
corrected.angular_velocity.y = message.angular_velocity.y
corrected.angular_velocity.z = -message.angular_velocity.z
corrected.orientation.x = 1
corrected.orientation.y = 0
corrected.orientation.z = 0
corrected.orientation.w = 0
corrected.orientation_covariance = [99999, 0, 0,
99999, .1, 0,
0, 0, 99999]
corrected.angular_velocity_covariance = [.000004, 0, 0,
0, .000004, 0,
0, 0, .000004]
corrected.linear_acceleration_covariance = [.000025, 0, 0,
0, .000025, 0,
0, 0, .000025]
pub = rospy.Publisher('imu_w_cov', Imu)
#print >> f, "%f %f %f %f %f %f \n" % (message.linear_acceleration.x, message.linear_acceleration.y, message.linear_acceleration.z, message.angular_velocity.x, message.angular_velocity.y, message.angular_velocity.z)
pub.publish(corrected)
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 cb_osc_accxyz(self, address_list, value_list, send_address):
"""
Callback for when accel data is received from a device.
Populates a sensor_msgs/Imu message, including the ip address of the
sending client in the msg.header.frame_id field.
@param address_list: A list with the OSC address parts in it.
@type address_list: C{list}
@param value_list: A list with the OSC value arguments in it.
@type value_list: C{list}
@param send_address: A tuple with the (ip, port) of the sender.
@type send_address: C{tuple}
"""
msg = Imu()
msg.linear_acceleration.x = value_list[0] * 9.80665
msg.linear_acceleration.y = value_list[1] * 9.80665
msg.linear_acceleration.z = value_list[2] * 9.80665
msg.header.frame_id = send_address[0]
msg.header.stamp = rospy.Time.now()
# Covariance was calculated from about 20 minutes of static data
# Conditions:
# * Back down
# * Plugged In
# * Vibrate Off
# * Cell and Wifi On
# Results:
# x y z
# Mean: 0.2934510093 -0.2174349315 -9.8049353269
# Stdev: 0.0197007054 0.0205649244 0.0259846818
# Var: 0.0003881178 0.0004229161 0.0006752037
var = 0.0008
msg.linear_acceleration_covariance = [var, 0, 0, 0, var, 0, 0, 0, var]
msg.angular_velocity_covariance = [0.0] * 9
msg.angular_velocity_covariance[0] = -1.0
msg.orientation_covariance = msg.angular_velocity_covariance
self.accel_pub.publish(msg)
def callback(self, data):
imu_message = Imu()
imu_message.header.stamp = rospy.Time.now()
imu_message.header.frame_id = "IMU"
imu_message.orientation.x = data.orientation.y
imu_message.orientation.y = data.orientation.x
imu_message.orientation.z = -data.orientation.z
imu_message.orientation.w = -data.orientation.w
imu_message.orientation_covariance = data.orientation_covariance
imu_message.linear_acceleration.x = data.linear_acceleration.y
imu_message.linear_acceleration.y = data.linear_acceleration.x
imu_message.linear_acceleration.z = -data.linear_acceleration.z
imu_message.linear_acceleration_covariance = data.linear_acceleration_covariance
imu_message.angular_velocity.x = data.angular_velocity.y
imu_message.angular_velocity.y = data.angular_velocity.x
imu_message.angular_velocity.z = -data.angular_velocity.z
imu_message.angular_velocity_covariance = data.linear_acceleration_covariance
self.pub.publish(imu_message)
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 publish_imu(self):
imu_msg = Imu()
imu_msg.header.stamp = self.time
imu_msg.header.frame_id = 'imu_odom'
a = self.kalman_state[0,0]
b = self.kalman_state[1,0]
c = self.kalman_state[2,0]
d = self.kalman_state[3,0]
imu_msg.orientation.x = a
imu_msg.orientation.y = b
imu_msg.orientation.z = c
imu_msg.orientation.w = d
imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
imu_msg.angular_velocity.x = 0
imu_msg.angular_velocity.y = 0
imu_msg.angular_velocity.z = 0
imu_msg.angular_velocity_covariance = list(zeros((3,3)).flatten())
imu_msg.linear_acceleration.x = self.measurement[0,0]
imu_msg.linear_acceleration.y = self.measurement[1,0]
imu_msg.linear_acceleration.z = self.measurement[2,0]
acc_cov = SF9DOF_UKF.measurement_noise(self.measurement, 1.0)[0:4][0:4]
imu_msg.linear_acceleration_covariance = list(acc_cov.flatten())
self.pub.publish(imu_msg)
