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 trigger(self):
cs = Compass()
# create frame id
cs.header.frame_id = self.frame_id
# create header (provides time stamp)
cs.header.stamp = rospy.Time.now()
# convert orientation from the CompassSensor.get_heading_lsb byte (heading in the 0-255 range)
# to quaternion
self.orientation = self.compass.get_sample()*-2.0 * math.pi/180.0
sample = self.compass.get_sample()
# create new imu msgs for the compass with time stamp as before
imu = Imu()
imu.header.frame_id = self.frame_id
imu.header.stamp = rospy.Time.now()
imu.angular_velocity.x = 0.0
imu.angular_velocity.y = 0.0
imu.angular_velocity.z = 1*math.pi/180.0 # the rotation speed
# covariance required by robot_pose_ekf
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orienation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
imu.orientation = cs.orientation
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub.publish(imu) # publish the message
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_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_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 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 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 trigger(self):
sample = self.gyro.get_sample()
gs = Gyro()
gs.header.frame_id = self.frame_id
gs.header.stamp = rospy.Time.now()
gs.calibration_offset.x = 0.0
gs.calibration_offset.y = 0.0
gs.calibration_offset.z = self.offset
gs.angular_velocity.x = 0.0
gs.angular_velocity.y = 0.0
gs.angular_velocity.z = (sample-self.offset)*math.pi/180.0
gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
self.pub.publish(gs)
imu = Imu()
imu.header.frame_id = self.frame_id
imu.header.stamp = rospy.Time.now()
imu.angular_velocity.x = 0.0
imu.angular_velocity.y = 0.0
imu.angular_velocity.z = 1*math.pi/180.0
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub2.publish(imu)
def _send_one(self, now, angle):
msg = Imu()
msg.header.stamp = now
msg.header.seq = self._seq
self._seq += 1
msg.header.frame_id = self._frame_id
msg.angular_velocity_covariance = np.zeros(9)
msg.orientation_covariance = np.zeros(9)
if self._gyro.current_mode == self._gyro.MODE_RATE:
msg.angular_velocity.z = angle - self._rate_bias
msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2
if self._invert_rotation:
msg.angular_velocity.z *= -1
if self._gyro.current_mode == self._gyro.MODE_DTHETA:
msg.angular_velocity = angle/self._sample_period - self._rate_bias
msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2
if self._invert_rotation:
msg.angular_velocity.z *= -1
if self._gyro.current_mode == self._gyro.MODE_INTEGRATED:
dt = (now - self._bias_measurement_time).to_sec()
corrected_angle = angle - self._rate_bias*dt
msg.orientation.w = cos(corrected_angle/2.0)
msg.orientation.z = sin(corrected_angle/2.0)
if self._invert_rotation:
msg.orientation.z *= -1
msg.orientation_covariance[8] = self._sigma_theta*self._sigma_theta
self._pub.publish(msg)
def run(self):
"""Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
The IMU message, if fully filled in, contains information on orientation,
acceleration (in m/s^2), and angular rate (in radians/sec). For each of
these quantities, the IMU message format also wants the corresponding
covariance matrix.
Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
data entry is marked invalid. We do this by setting the first
entry of its associated covariance matrix to -1. The covariance
matrices are the 3x3 matrix with the axes' variance in the
diagonal. We obtain the variance from the Wiimote instance.
"""
rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
self.threadName = "IMU topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
msg = Imu(header=None,
orientation=None, # will default to [0.,0.,0.,0],
orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.], # -1 indicates that orientation is unknown
angular_velocity=None,
angular_velocity_covariance=self.angular_velocity_covariance,
linear_acceleration=None,
linear_acceleration_covariance=self.linear_acceleration_covariance)
# If a gyro is plugged into the Wiimote, then note the
# angular velocity in the message, else indicate with
# the special gyroAbsence_covariance matrix that angular
# velocity is unavailable:
if self.wiistate.motionPlusPresent:
msg.angular_velocity.x = canonicalAngleRate[PHI]
msg.angular_velocity.y = canonicalAngleRate[THETA]
msg.angular_velocity.z = canonicalAngleRate[PSI]
else:
msg.angular_velocity_covariance = self.gyroAbsence_covariance
msg.linear_acceleration.x = canonicalAccel[X]
msg.linear_acceleration.y = canonicalAccel[Y]
msg.linear_acceleration.z = canonicalAccel[Z]
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
self.pub.publish(msg)
rospy.logdebug("IMU state:")
rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate))
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Imu sender.")
exit(0)
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 spin(self):
self.prev_time = rospy.Time.now()
while not rospy.is_shutdown():
if self.calibrating:
self.calibrate()
self.calibrating = False
self.prev_time = rospy.Time.now()
acceldata = self.accelerometer.read6Reg(accel_x_low)
compassdata = self.compass.read6Reg(compass_x_high)
gyrodata = self.gyro.read6Reg(gyro_x_low)
# prepare Imu frame
imu = Imu()
imu.header.frame_id = self.frame_id
self.linear_acceleration = Vector3();
# get line from device
#str = self.ser.readline()
# timestamp
imu.header.stamp = rospy.Time.now()
#nums = str.split()
# check, if it was correct line
#if (len(nums) != 5):
# continue
self.linear_acceleration.x = self.twosComplement(acceldata[0], acceldata[1]) #/16384.0
self.linear_acceleration.y = self.twosComplement(acceldata[2], acceldata[3]) #/16384.0
self.linear_acceleration.z = self.twosComplement(acceldata[4], acceldata[5]) #/16384.0
imu.orientation.x = self.twosComplement(compassdata[1], compassdata[0]) #/1055.0,
imu.orientation.y = self.twosComplement(compassdata[3], compassdata[2]) #/1055.0,
imu.orientation.z = self.twosComplement(compassdata[5], compassdata[4]) #/950.0
imu.angular_velocity.x = self.twosComplement(gyrodata[0], gyrodata[1])
imu.angular_velocity.y = self.twosComplement(gyrodata[2], gyrodata[3])
imu.angular_velocity.z = self.twosComplement(gyrodata[4], gyrodata[5])
#gyro = int(nums[2])
#ref = int(nums[3])
#temp = int(nums[4])
#val = (ref-gyro - self.bias) * 1000 / 3 / 1024 * self.scale
#imu.angular_velocity.x = 0
#imu.angular_velocity.y = 0
#imu.angular_velocity.z = val * math.pi / 180
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub.publish(imu)
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 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 main():
rospy.init_node(NODE_NAME)
params = rospy.get_param("/imu".format(NODE_NAME),
{
'gyro': {
'zero':[-129.91, 63.81, -102.36],
'covariance':[0,0,0,
0,3.6935414044e-06,0,
0,0,0]
},
'angle':{
'zero':[0,-0.118467263978,0],
'covariance':[0,0,0,
0,3.57350008404e-05,0,
0,0,0]
}
})
rospy.set_param("/imu", params)
gyro_zeros = np.array(params['gyro']['zero'])
gyro_y_variance = params['gyro']['covariance'][4]
angle_y_zero = params['angle']['zero'][Y]
angle_y_variance = params['angle']['covariance'][4]
rate = rospy.Rate(SAMPLE_FREQ_HZ)
publisher = rospy.Publisher(IMU_TOPIC, Imu, queue_size=1, tcp_nodelay=True)
raw_publisher = rospy.Publisher(IMU_RAW_TOPIC, Imu, queue_size=1, tcp_nodelay=True)
rospy.loginfo("Starting {}, publishing at {} hz on {}".format(NODE_NAME, SAMPLE_FREQ_HZ, IMU_TOPIC))
state = Sample()
work = Sample()
sensor_reading = Sample()
raw_msg = Imu()
imu_msg = Imu()
imu_msg.angular_velocity_covariance = params['gyro']['covariance']
# imu_msg.linear_acceleration_covariance = params['accel']['covariance']
while not rospy.is_shutdown():
read_sensor(sensor_reading)
if PUBLISH_RAW:
populate_message(raw_msg, sensor_reading)
raw_publisher.publish(raw_msg)
process_reading(sensor_reading, work, state, gyro_zeros, gyro_y_variance, angle_y_zero, angle_y_variance)
populate_message(imu_msg, state)
publisher.publish(imu_msg)
rate.sleep()
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 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 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 convertData(self):
if self.msgid < self.msgs_len:
msg = Imu()
msg.header.seq = self.timestamp_ms[self.msgid][0]
msg.header.stamp = rospy.Time.from_sec(self.stamp)
# TODO:
# SCALED_IMU.msg:int16
# HIGHRES_IMU.msg:float32
# RAW_IMU.msg:int16
msg.angular_velocity.x = self.getMsgValue("GyrX")
msg.angular_velocity.y = self.getMsgValue("GyrY")
msg.angular_velocity.z = self.getMsgValue("GyrZ")
msg.linear_acceleration.x = self.getMsgValue("AccX")
msg.linear_acceleration.y = self.getMsgValue("AccY")
msg.linear_acceleration.z = self.getMsgValue("AccZ")
# TODO: This is not enough for ROS IMU msg, add cov matrices and calculate orientation
# simple complementary filter for orientation
GYROSCOPE_SENSITIVITY = 1
# dt = self.last_stamp - self.stamp
#pitch += (msg.angular_velocity.x / GYROSCOPE_SENSITIVITY) * dt # Angle around the X-axis
#roll -= (msg.angular_velocity.y / GYROSCOPE_SENSITIVITY) * dt # Angle around the Y-axis
# Turning around the X axis results in a vector on the Y-axis
#pitchAcc = atan2(msg.linear_acceleration.x, ( sqrt(pow(msg.linear_acceleration.y,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI
#pitch = pitch * 0.98 + pitchAcc * 0.02
# Turning around the Y axis results in a vector on the X-axis
#rollAcc = atan2(msg.linear_acceleration.y, (sqrt(pow(msg.linear_acceleration.x,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI
#roll = roll * 0.98 + rollAcc * 0.02
#yaw = (msg.angular_velocity.z / GYROSCOPE_SENSITIVITY) * dt
#orientation.setRPY(roll, pitch, yaw)
#tf::quaternionTFToMsg(orientation, orientation_msg)
#msg.header.frame_id = frame_id_imu_link;
#imu_transform.setRotation(orientation);
msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
msg.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
#self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
#self.prev_time = imu.header.stamp
#(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.bag.write(self.topic, msg, msg.header.stamp)
self.msgid = self.msgid + 1
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 pub_imu(msg_type, msg, bridge):
pub = bridge.get_ros_pub("imu", Imu, queue_size=1)
imu = Imu()
imu.header.stamp = bridge.project_ap_time(msg)
imu.header.frame_id = 'base_footprint'
# Orientation as a Quaternion, with unknown covariance
quat = quaternion_from_euler(msg.roll, msg.pitch, msg.yaw, 'sxyz')
imu.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
imu.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
# Angular velocities, with unknown covariance
imu.angular_velocity.x = msg.rollspeed
imu.angular_velocity.y = msg.pitchspeed
imu.angular_velocity.z = msg.yawspeed
imu.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
# Not supplied with linear accelerations
imu.linear_acceleration_covariance[0] = -1
pub.publish(imu)
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 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
# 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
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pub_imu_pendu_mag = rospy.Publisher('imu_pendu_mag', Vector3Stamped, queue_size = 1)
imu_plate_Msg = Imu()
imu_plate_mag_Msg = Vector3Stamped()
imu_pendu_Msg = Imu()
imu_pendu_mag_Msg = Vector3Stamped()
imu_plate_Msg.orientation_covariance = [
0.0025 , 0 , 0,
0, 0.0025, 0,
0, 0, 0.0025
]
imu_plate_Msg.angular_velocity_covariance = [
0.02, 0 , 0,
0 , 0.02, 0,
0 , 0 , 0.02
]
imu_plate_Msg.linear_acceleration_covariance = [
0.04 , 0 , 0,
0 , 0.04, 0,
0 , 0 , 0.04
]
imu_pendu_Msg.orientation_covariance = [
0.0025 , 0 , 0,
0, 0.0025, 0,
0, 0, 0.0025
]
def start(self):
imu_count = 0
speed_count = 0
gnss_count = 0
time = 0
total_time = self.imu_data["t"][0][0][-1][0]
while ((not rospy.is_shutdown()) and time < total_time):#checking if the IMU has stopped publishin
time = self.imu_data["t"][0][0][imu_count][0]
#sending IMU data
acc = self.imu_data["acc"][0][0][0:, imu_count]
gyro = self.imu_data["gyro"][0][0][0:, imu_count]
imuMsg = Imu()
imuMsg.header.stamp.secs = time
imuMsg.header.frame_id = "base_link"
#defining the covariance matrix for IMU measurement
imuMsg.angular_velocity_covariance = [
0.002, 0, 0,
0, 0.002, 0,
0, 0, 0.002
]
imuMsg.linear_acceleration_covariance = [
0.08, 0, 0,
0, 0.08, 0,
0, 0, 0.08
]
imuMsg.angular_velocity.x = gyro[1]
imuMsg.angular_velocity.y = gyro[0]
imuMsg.angular_velocity.z = -gyro[2]
imuMsg.linear_acceleration.x = acc[1]
imuMsg.linear_acceleration.y = acc[0]
imuMsg.linear_acceleration.z = -acc[2]
self.pub_imu.publish(imuMsg)
imu_count = imu_count + 1
# sending Odom data
if(self.speedometer_data["t"][0][0][speed_count][0] < time):
odomMsg = Odometry()
odomMsg.header.stamp.secs = self.speedometer_data["t"][0][0][speed_count][0]
odomMsg.header.frame_id = "odom"
odomMsg.child_frame_id = "base_link"
odomMsg.twist.twist.linear.x = self.speedometer_data["speed"][0][0][0][speed_count]
odomMsg.twist.twist.linear.y = 0.0
self.pub_odom.publish(odomMsg)
speed_count = speed_count + 1
#sending Map data
if (self.gnss_data["t"][0][0][gnss_count][0] < time):
gnssMsg = Odometry()
ned_pos = self.gnss_data["pos_ned"][0][0][0:, gnss_count]
gnssMsg.header.stamp.secs = self.gnss_data["t"][0][0][gnss_count][0]
gnssMsg.header.frame_id = "map"
gnssMsg.pose.pose.position.x = ned_pos[1]
gnssMsg.pose.pose.position.y = ned_pos[0]
gnssMsg.pose.pose.position.z = 0.0
self.pub_gnss.publish(gnssMsg)
gnss_count = gnss_count + 1
self.looprate.sleep()
def publishState(self, state, ros_pub_dec, numDoF):
# Publish our robot state to ROS topics /robotname/state/* periodically
self.publish_time += 1
if bROS and self.publish_time > ros_pub_dec:
publish_time = 0
# Construct /robotname/state/imu ROS message
msg = Imu()
msg.linear_acceleration.x = state['imu/linear_acceleration'][0]
msg.linear_acceleration.y = state['imu/linear_acceleration'][1]
msg.linear_acceleration.z = state['imu/linear_acceleration'][2]
msg.angular_velocity.x = state['imu/angular_velocity'][0]
msg.angular_velocity.y = state['imu/angular_velocity'][1]
msg.angular_velocity.z = state['imu/angular_velocity'][2]
msg.orientation_covariance = state['imu/orientation_covariance']
msg.linear_acceleration_covariance = np.empty(9)
msg.linear_acceleration_covariance.fill(
state['imu/linear_acceleration_covariance'])
msg.angular_velocity_covariance = np.empty(9)
msg.angular_velocity_covariance.fill(
state['imu/angular_velocity_covariance'])
roll, pitch, yaw = state[
'imu/euler'] # Convert from euler to quaternion using ROS tf
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
msg.orientation.x = quaternion[0]
msg.orientation.y = quaternion[1]
msg.orientation.z = quaternion[2]
msg.orientation.w = quaternion[3]
self.pubs[0].publish(msg)
# Construct /robotname/state/pose
msg = Pose()
msg.orientation.x = quaternion[0]
msg.orientation.y = quaternion[1]
msg.orientation.z = quaternion[2]
msg.orientation.w = quaternion[3]
# TODO: Get height from robot state, have robot calculate it
msg.position.z = 0.0
self.pubs[7].publish(msg)
# Construct /robotname/state/batteryState
msg = BatteryState()
msg.current = state['battery/current']
msg.voltage = state['battery/voltage']
#num_cells = 8
num_cells = 4
if 'battery/cell_voltage' in state:
if state['battery/cell_voltage'] > 0:
num_cells = len(state['battery/cell_voltage'])
# FIXME: do i really need this?
def percentage(total_voltage, num_cells):
# Linearly interpolate charge from voltage
# https://gitlab.com/ghostrobotics/SDK/uploads/6878144fa0e408c91e481c2278215579/image.png
charges = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0]
voltages = [3.2, 3.5, 3.6, 3.65, 3.8, 4.2]
return np.interp(total_voltage / num_cells, voltages, charges)
if msg.percentage <= 0:
msg.percentage = percentage(msg.voltage, num_cells)
self.pubs[1].publish(msg)
# Construct /robotname/state/behaviorId
msg = UInt32()
msg.data = state['behaviorId']
self.pubs[2].publish(msg)
# Construct /robotname/state/behaviorMode
msg = UInt32()
msg.data = state['behaviorMode']
self.pubs[3].publish(msg)
# Construct /robotname/state/joint
msg = JointState()
msg.name = []
msg.position = []
msg.velocity = []
msg.effort = []
for j in range(len(state['joint/position'])):
msg.name.append(str(j))
msg.position.append(state['joint/position'][j])
msg.velocity.append(state['joint/velocity'][j])
msg.effort.append(state['joint/effort'][j])
self.pubs[4].publish(msg)
msg.position = convert_to_leg_model(msg.position)
# Translate for URDF in NGR
# vision60 = False
# if(vision60):
# for i in range(8, 2):
# msg.position[i] += msg.position[i-1];
# msg.velocity[i] += msg.velocity[i-1];
# else:
# # other URDF
# # for URDF of Minitaur FIXME use the class I put in ethernet.py for RobotType
# msg.position.extend([0, 0, 0, 0, 0, 0, 0, 0])
# msg.position[11], msg.position[10], r = minitaurFKForURDF(msg.position[0], msg.position[1])
# msg.position[14], msg.position[15], r = minitaurFKForURDF(msg.position[2], msg.position[3])
# msg.position[9], msg.position[8], r = minitaurFKForURDF(msg.position[4], msg.position[5])
# msg.position[13], msg.position[12], r = minitaurFKForURDF(msg.position[6], msg.position[7])
# # other URDF problems (order important)
# for j in range(4):
# msg.position[j] = -msg.position[j]
self.pubs[5].publish(msg)
# Construct /robotname/state/joystick
msg = Joy()
msg.axes = state['joy/axes']
msg.buttons = state['joy/buttons']
self.pubs[6].publish(msg)
imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
reset()
# Try sleeping for some time before running calibrate to avoid sudden wheel movement
# interfering with calibration.
sleep(0.2)
# Run a calibration before starting to send data to the robot.
cal_values = calibrate()
imu_msg = Imu()
imu_msg.angular_velocity_covariance = [
0, 0, 0, 0, 0, 0, 0, 0,
get_variance('z', cal_values)
]
imu_msg.linear_acceleration_covariance = [
get_variance('x', cal_values),
get_covariance('x', cal_values), 0,
get_covariance('y', cal_values),
get_variance('y', cal_values), 0, 0, 0, 0
]
idx = 0
rawdata = []
sensor_data = [0, 0, 0]
rate = rospy.Rate(50)
def talker():
orientation_covariance = [0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity_covariance = [0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration_covariance = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
publisher_left = rospy.Publisher('wheel_speed_left', Int32, queue_size=1)
publisher_right = rospy.Publisher('wheel_speed_right', Int32, queue_size=1)
publisher_twist = rospy.Publisher('wheel_speed_twist', Twist, queue_size=1)
publisher_sonar = rospy.Publisher('data_sonar', Vector3, queue_size=1)
publisher_bumper = rospy.Publisher('data_bumper', Vector3, queue_size=1)
publisher_imu = rospy.Publisher('data_imu', Imu, queue_size=1)
# publisher_imu_angular_velocities = rospy.Publisher(
# 'data_imu_angular_velocities', Vector3, queue_size=1)
# publisher_imu_accelerations = rospy.Publisher(
# 'data_imu_accelerations', Vector3, queue_size=1)
# publisher_imu_quaternion = rospy.Publisher(
# 'data_imu_quaternion', Quaternion, queue_size=1)
rospy.init_node('publish_robot_data')
ser = open_port()
print(ser)
with ser:
while not rospy.is_shutdown():
line = ser.readline().rstrip()
if len(line) > 4:
if line[0] == 'W' and line[1] == '['\
and line[len(line) - 1] == 'W'\
and line[len(line) - 2] == ']':
parsed_values = parse_data_wheels(line[2:len(line) - 2])
if parsed_values[0]:
publisher_left.publish(parsed_values[2])
publisher_right.publish(parsed_values[3])
publisher_twist.publish(parsed_values[1])
elif line[0] == 'S' and line[1] == '['\
and line[len(line) - 1] == 'S'\
and line[len(line) - 2] == ']':
parsed_values = parse_data_sonar(line[2:len(line) - 2])
if parsed_values[0]:
publisher_sonar.publish(parsed_values[1])
elif line[0] == 'B' and line[1] == '['\
and line[len(line) - 1] == 'B'\
and line[len(line) - 2] == ']':
parsed_values = parse_data_bumper(line[2:len(line) - 2])
if parsed_values[0]:
publisher_bumper.publish(parsed_values[1])
elif line[0] == 'I' and line[1] == '['\
and line[len(line) - 1] == 'I'\
and line[len(line) - 2] == ']':
parsed_values = parse_data_imu(line[2:len(line) - 2])
if parsed_values[0]:
imu_angular_velocities = parsed_values[1]
imu_accelerations = parsed_values[2]
imu_quaternion = parsed_values[3]
imu_message = Imu()
now = rospy.Time.now()
imu_message.header.stamp = now
imu_message.header.frame_id = 'imu'
imu_message.angular_velocity = imu_angular_velocities
imu_message.linear_acceleration = imu_accelerations
imu_message.orientation = imu_quaternion
imu_message.linear_acceleration_covariance = linear_acceleration_covariance
imu_message.angular_velocity_covariance = angular_velocity_covariance
imu_message.orientation_covariance = orientation_covariance
publisher_imu.publish(imu_message)
# publisher_imu_angular_velocities.publish(
# parsed_values[1])
# publisher_imu_accelerations.publish(parsed_values[2])
# publisher_imu_quaternion.publish(parsed_values[3])
else:
print("Cannot parse data from the string")
else:
print("Received string is empty")
def main(argv):
if len(sys.argv) < 2:
print 'Please specify data directory file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
print("loading files...")
bag = rosbag.Bag(sys.argv[2], 'w')
gps = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/gps.csv",
delimiter=",")
imu_100hz = np.loadtxt(sys.argv[1] +
"sensor_data/2012-01-08/imu_100hz.csv",
delimiter=",")
ral_seq = 0
bap_seq = 0
img_seq = 0
imu_seq = 0
cal = -1
gps_seq = 0
# IMAGE_COUNT = 81169
STREET_VIEW = 113
print("package gps and image...")
print("Packaging GPS and image")
for gps_data in gps:
utime = int(gps_data[0])
mode = int(gps_data[1])
timestamp = rospy.Time.from_sec(utime / 1e6)
lat = float(gps_data[3])
lng = float(gps_data[4])
alt = float(gps_data[5])
status = NavSatStatus()
if mode == 0 or mode == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = float(gps_data[2])
fix = NavSatFix()
fix.header.seq = gps_seq
fix.status = status
fix.latitude = np.rad2deg(lat)
fix.longitude = np.rad2deg(lng)
fix.altitude = np.rad2deg(alt)
track = Float64()
track.data = float(gps_data[6])
speed = Float64()
speed.data = float(gps_data[7])
bag.write('/gps', fix, t=timestamp)
bag.write('/gps_track', track, t=timestamp)
bag.write('/gps_speed', speed, t=timestamp)
# write aerial image
if gps_seq <= MAVIMAGE:
img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam5/'
img_list = os.listdir(img_path)
img_list.sort()
img_cv = cv2.imread(os.path.join(img_path, img_list[gps_seq]), -1)
img_cv = cv2.resize(img_cv, MAVDIM, interpolation=cv2.INTER_AREA)
# 顺时针旋转90度
trans_img = cv2.transpose(img_cv)
img_cv = cv2.flip(trans_img, 1)
br = CvBridge()
Img = Image()
Img = br.cv2_to_imgmsg(img_cv, "bgr8")
Img.header.seq = int(gps_seq)
print(gps_seq)
Img.header.stamp = timestamp
Img.header.frame_id = 'camera'
bag.write('/image/cam5', Img, t=timestamp)
gps_seq = gps_seq + 1
print('packaging imu...')
for imu_data in imu_100hz:
imu_seq = imu_seq + 1
utime = int(imu_data[0])
timestamp = rospy.Time.from_sec(utime / 1e6)
imu = Imu()
imu.header.seq = imu_seq
imu.header.stamp = timestamp
imu.header.frame_id = '/Imu'
imu.linear_acceleration.x = float(imu_data[5])
imu.linear_acceleration.y = float(imu_data[6])
imu.linear_acceleration.z = float(imu_data[7])
imu.linear_acceleration_covariance = np.zeros(9)
imu.angular_velocity.x = float(imu_data[8])
imu.angular_velocity.y = float(imu_data[9])
imu.angular_velocity.z = float(imu_data[10])
imu.angular_velocity_covariance = np.zeros(9)
imu.orientation.w = float(imu_data[1])
imu.orientation.x = float(imu_data[2])
imu.orientation.y = float(imu_data[3])
imu.orientation.z = float(imu_data[4])
bag.write('/Imu', imu, t=timestamp)
bag.close()
return 0
def imunode():
global freq
global accel, gyro
global R, q, rpyi
R = np.eye(3)
q = np.array([0, 0, 0, 1])
rpy = np.array([[0], [0], [0]])
imu_msg = Imu()
imu_msg.header.frame_id = "/imu"
#imu_msg.header.frame_id = "world"
odom_msg = Odometry()
odom_msg.header.frame_id = "world"
odom_msg.child_frame_id = "EspeleoRobo"
gps_msg = NavSatFix()
gps_msg.header.frame_id = "/imu"
pub_imu = rospy.Publisher("/imu/raw", Imu, queue_size=1)
pub_gps = rospy.Publisher("/fix", NavSatFix, queue_size=1)
rospy.init_node("xsens_emulator")
rospy.Subscriber("/sensors/acc", Point, callback_acc)
rospy.Subscriber("/sensors/gyro", Point, callback_gyro)
rospy.Subscriber("/tf", TFMessage,
callback_pose_tf) # ground thruth - from tf transform
rate = rospy.Rate(freq)
sleep(0.5)
print "\33[92mXsens emulator initialized!\33[0m"
i = 0
while not rospy.is_shutdown():
i = i + 1
time = i / float(freq)
#Publish imu data
imu_msg.header.stamp = rospy.Time.now()
imu_msg.angular_velocity.x = gyro[0]
imu_msg.angular_velocity.y = gyro[1]
imu_msg.angular_velocity.z = gyro[2]
imu_msg.angular_velocity_covariance = [
0.2, 0.1, 0.1, 0.1, 0.2, 0.1, 0.1, 0.1, 0.2
]
imu_msg.linear_acceleration.x = accel[0]
imu_msg.linear_acceleration.y = accel[1]
imu_msg.linear_acceleration.z = accel[2]
imu_msg.linear_acceleration_covariance = [
0.2, 0.1, 0.1, 0.1, 0.2, 0.1, 0.1, 0.1, 0.2
]
pub_imu.publish(imu_msg)
# ---------- ---------- ---------- ---------- ----------
#Publish gps data
gps_msg.header.stamp = rospy.Time.now()
gps_msg.latitude = gps[1]
gps_msg.longitude = gps[0]
gps_msg.altitude = gps[2]
pub_gps.publish(gps_msg)
# ---------- ---------- ---------- ---------- ----------
"""
# Create odom message
odom_msg.header.stamp = rospy.Time.now();
#odom_msg.pose.pose.position.x = p[0]
#odom_msg.pose.pose.position.y = p[1]
#odom_msg.pose.pose.position.z = p[2]
odom_msg.pose.pose.orientation.x = q[0]
odom_msg.pose.pose.orientation.y = q[1]
odom_msg.pose.pose.orientation.z = q[2]
odom_msg.pose.pose.orientation.w = q[3]
#Publish odom message
pub_odom.publish(odom_msg)
"""
rate.sleep()
#Sensors data ROS buffers
orientation_msg = Vector3(0, 0, 0)
angular_vel_msg = Vector3(0, 0, 0)
lin_accel_msg = Vector3(0, 0, 0)
#Sensors data dictionary
sensor_data_dict = {
'A': lin_accel_msg,
'G': angular_vel_msg,
'C': orientation_msg
}
#ROS IMU message
imuMsg = Imu()
imuMsg.orientation_covariance = orientation_covariance
imuMsg.angular_velocity_covariance = angular_velocity_covariance
imuMsg.linear_acceleration_covariance = linear_acceleration_covariance
imuMsg.orientation = orientation_msg
imuMsg.angular_velocity = angular_vel_msg
imuMsg.linear_acceleration = lin_accel_msg
"""
Node must be able of receiving STM32 sensor messages through
serial port, and transform them to a ROS IMU message. The data
received through the serial port has the following text format:
<Type> ':' <Data> <Data> ... <Data> '\r' '\n'
"Type" must be one of these letters: A (accelerometer), C (compass),
G (gyroscope).
"""
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
#COLLIN modified
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']
imu_msg.linear_acceleration_covariance = [
5e-2, 0, 0, 0, 5e-2, 0, 0, 0, 5e-2
]
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']
#COLLIN modified
imu_msg.angular_velocity_covariance = [
5e-2, 0, 0, 0, 5e-2, 0, 0, 0, 5e-2
]
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.w = orient_data['Q0']
imu_msg.orientation.x = orient_data['Q1']
imu_msg.orientation.y = orient_data['Q2']
imu_msg.orientation.z = 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)
S_Compass_Declination = rospy.get_param('~declination',0.2333333333334*(math.pi/180.0))
S_Compass_EastAsZero = rospy.get_param('~UseEastAsZero',True)
Checksum_error_limits = rospy.get_param('~Checksum_error_limits', 15)
checksum_error_counter=0
#-------------------------------------------------------------------------------------------------------------
imu_data = Imu()
imu_data = Imu(header=rospy.Header(frame_id="SpartonCompassIMU"))
# Initialize Imu message
imu_data.orientation_covariance = [1e-6, 0, 0,
0, 1e-6, 0,
0, 0, 1e-6]
imu_data.angular_velocity_covariance = [1e-6, 0, 0,
0, 1e-6, 0,
0, 0, 1e-6]
imu_data.linear_acceleration_covariance = [1e-6, 0, 0,
0, 1e-6, 0,
0, 0, 1e-6]
# Instruction lines
Dropline='\r\n\r\n print trigger 0 set drop \r\n'
Checkline='\r\n printmask gyrop_trigger accelp_trigger or quat_trigger or time_trigger or set drop \r\n'
ModulusLine='printmodulus %i set drop \r\n' % S_Printmodulus
Streamline='printtrigger printmask set drop \r\n'
GSRline='$PSRFS,gyroSampleRate,get'
rospy.on_shutdown(Shutdown)
try:
#!/usr/bin/env python
import roslib
import rospy
from crius_imu.msg import ImuMin
from sensor_msgs.msg import Imu
rospy.init_node('imuraw2imu_node')
imu_msg = Imu()
imu_msg.orientation_covariance = (1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0)
imu_msg.angular_velocity_covariance = (1.25e-07, 0, 0, 0, 1.25e-07, 0, 0, 0,
1.25e-07)
imu_msg.linear_acceleration_covariance = (8.99e-08, 0, 0, 0, 8.99e-08, 0, 0, 0,
8.99e-08)
pub = rospy.Publisher('/imu/data', Imu, queue_size=1)
def callback(data):
imu_msg.header.stamp = rospy.get_rostime()
imu_msg.header.frame_id = "imu"
imu_msg.orientation.x = data.quat_x
imu_msg.orientation.y = data.quat_y
imu_msg.orientation.z = data.quat_z
imu_msg.orientation.w = data.quat_w
imu_msg.angular_velocity.x = data.gyro_x
imu_msg.angular_velocity.y = data.gyro_y
imu_msg.angular_velocity.z = data.gyro_z
imu_msg.linear_acceleration.x = data.acc_x
imu_msg.linear_acceleration.y = data.acc_y
imu_msg.linear_acceleration.z = data.acc_z
pub.publish(imu_msg)
def __init__(self):
self.DEVICE_ID = rospy.get_param(
'~device_id', '/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00')
self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size=1)
self.mag_pub = rospy.Publisher('imu/camera_mag',
MagneticField,
queue_size=1)
imu = Imu()
mag = MagneticField()
ser = serial.Serial(self.DEVICE_ID)
gyro = [0.0, 0.0, 0.0, 0.0]
magn = [0.0, 0.0, 0.0, 0.0]
quat = [0.0, 1.0, 0.0, 0.0, 0.0]
rate = rospy.Rate(130)
while not rospy.is_shutdown():
line = ser.readline()
accel = line.split(",", 4)
if accel[0] == 'A':
line = ser.readline()
gyro = line.split(",", 4)
line = ser.readline()
magn = line.split(",", 4)
line = ser.readline()
quat = line.split(",", 5)
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = '/base_link' # i.e. '/odom'
#pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint'
imu.angular_velocity.x = float(gyro[1])
imu.angular_velocity.y = float(gyro[2])
imu.angular_velocity.z = float(gyro[3])
imu.angular_velocity_covariance = [
.001, 0.0, 0.0, 0.0, .001, 0.0, 0.0, 0.0, .001
]
imu.linear_acceleration.x = float(accel[1])
imu.linear_acceleration.y = float(accel[2])
imu.linear_acceleration.z = float(accel[3])
imu.linear_acceleration_covariance = [
.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01
]
'''imu.orientation.w = float(quat[1])
imu.orientation.x = float(quat[2])
imu.orientation.y = float(quat[3])
imu.orientation.z = float(quat[4])
imu.orientation_covariance = [.01, 0.0, 0.0,
0.0, .01, 0.0,
0.0, 0.0, .01] '''
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = '/base_link' # i.e. '/odom'
#pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint'
mag.magnetic_field.x = float(magn[1])
mag.magnetic_field.y = float(magn[2])
mag.magnetic_field.z = float(magn[3])
mag.magnetic_field_covariance = [
.001, 0.0, 0.0, 0.0, .001, 0.0, 0.0, 0.0, .001
]
self.imu_pub.publish(imu)
self.mag_pub.publish(mag)
rate.sleep()
def loop(self):
#Topic 1: PoseWitchCovariance
pwc = PoseWithCovarianceStamped()
pwc.header.stamp = rospy.get_rostime()
pwc.header.frame_id = self.world_frame_id
pwc.pose.pose.position = Coordinates()
pwc.pose.pose.orientation = pypozyx.Quaternion()
cov = pypozyx.PositionError()
status = self.pozyx.doPositioning(pwc.pose.pose.position,
self.dimension, self.height,
self.algorithm, self.tag_device_id)
pozyx.getQuaternion(pwc.pose.pose.orientation, self.tag_device_id)
pozyx.getPositionError(cov, self.tag_device_id)
cov_row1 = [cov.x, cov.xy, cov.xz, 0, 0, 0]
cov_row2 = [cov.xy, cov.y, cov.yz, 0, 0, 0]
cov_row3 = [cov.xz, cov.yz, cov.z, 0, 0, 0]
cov_row4 = [0, 0, 0, 0, 0, 0]
cov_row5 = [0, 0, 0, 0, 0, 0]
cov_row6 = [0, 0, 0, 0, 0, 0]
pwc.pose.covariance = cov_row1 + cov_row2 + cov_row3 + cov_row4 + cov_row5 + cov_row6
#Convert from mm to m
pwc.pose.pose.position.x = pwc.pose.pose.position.x * 0.001
pwc.pose.pose.position.y = pwc.pose.pose.position.y * 0.001
pwc.pose.pose.position.z = pwc.pose.pose.position.z * 0.001
if status == POZYX_SUCCESS:
pub_pose_with_cov.publish(pwc)
#Topic 2: IMU
imu = Imu()
imu.header.stamp = rospy.get_rostime()
imu.header.frame_id = self.tag_frame_id
imu.orientation = pypozyx.Quaternion()
imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.angular_velocity = pypozyx.AngularVelocity()
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.linear_acceleration = pypozyx.LinearAcceleration()
imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
pozyx.getQuaternion(imu.orientation, self.tag_device_id)
pozyx.getAngularVelocity_dps(imu.angular_velocity, self.tag_device_id)
pozyx.getLinearAcceleration_mg(imu.linear_acceleration,
self.tag_device_id)
#Convert from mg to m/s2
imu.linear_acceleration.x = imu.linear_acceleration.x * 0.0098
imu.linear_acceleration.y = imu.linear_acceleration.y * 0.0098
imu.linear_acceleration.z = imu.linear_acceleration.z * 0.0098
#Convert from Degree/second to rad/s
imu.angular_velocity.x = imu.angular_velocity.x * 0.01745
imu.angular_velocity.y = imu.angular_velocity.y * 0.01745
imu.angular_velocity.z = imu.angular_velocity.z * 0.01745
pub_imu.publish(imu)
#Topic 3: Anchors Info
for i in range(len(anchors)):
dr = AnchorInfo()
dr.header.stamp = rospy.get_rostime()
dr.header.frame_id = self.world_frame_id
dr.id = hex(anchors[i].network_id)
dr.position.x = (float)(anchors[i].pos.x) * 0.001
dr.position.y = (float)(anchors[i].pos.y) * 0.001
dr.position.z = (float)(anchors[i].pos.z) * 0.001
iter_ranging = 0
while iter_ranging < self.do_ranging_attempts:
device_range = DeviceRange()
status = self.pozyx.doRanging(self.anchors[i].network_id,
device_range, self.tag_device_id)
dr.distance = (float)(device_range.distance) * 0.001
dr.RSS = device_range.RSS
if status == POZYX_SUCCESS:
dr.status = True
self.range_error_counts[i] = 0
iter_ranging = self.do_ranging_attempts
else:
dr.status = False
self.range_error_counts[i] += 1
if self.range_error_counts[i] > 9:
self.range_error_counts[i] = 0
rospy.logerr("Anchor %d (%s) lost", i, dr.id)
iter_ranging += 1
# device_range = DeviceRange()
# status = self.pozyx.doRanging(self.anchors[i].network_id, device_range)
# dr.distance = (float)(device_range.distance) * 0.001
# dr.RSS = device_range.RSS
# if status == POZYX_SUCCESS:
# dr.status = True
# self.range_error_counts[i] = 0
# else:
# status = self.pozyx.doRanging(self.anchors[i].network_id, device_range)
# dr.distance = (float)(device_range.distance) * 0.001
# dr.RSS = device_range.RSS
# if status == POZYX_SUCCESS:
# dr.status = True
# self.range_error_counts[i] = 0
# else:
# dr.status = False
# self.range_error_counts[i] += 1
# if self.range_error_counts[i] > 9:
# self.range_error_counts[i] = 0
# rospy.logerr("Anchor %d (%s) lost", i, dr.id)
dr.child_frame_id = "anchor_" + str(i)
pub_anchor_info[i].publish(dr)
#Topic 4: PoseStamped
ps = PoseStamped()
ps.header.stamp = rospy.get_rostime()
ps.header.frame_id = self.world_frame_id
ps.pose.position = pwc.pose.pose.position
ps.pose.orientation = pwc.pose.pose.orientation
pub_pose.publish(ps)
#Topic 5: Pressure
pr = FluidPressure()
pr.header.stamp = rospy.get_rostime()
pr.header.frame_id = self.tag_frame_id
pressure = pypozyx.Pressure()
pozyx.getPressure_Pa(pressure, self.tag_device_id)
pr.fluid_pressure = pressure.value
pr.variance = 0
pub_pressure.publish(pr)
imuMsg.orientation.w = real
if mag_accuracy == 0:
imuMsg.orientation_covariance = [
0.0001, 0.00, 0.00, 0.00, 0.0001, 0.00, 0.00, 0.00, 0.0001
]
else:
imuMsg.orientation_covariance = [
0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01
]
# Gyroscope data (in degrees per second):
imuMsg.angular_velocity.x = gyroX
imuMsg.angular_velocity.y = gyroY
imuMsg.angular_velocity.z = gyroZ
imuMsg.angular_velocity_covariance = [
0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01
]
# Accelerometer data (in meters per second squared):
imuMsg.linear_acceleration.x = accelX
imuMsg.linear_acceleration.y = accelY
imuMsg.linear_acceleration.z = accelZ
imuMsg.linear_acceleration_covariance = [
0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01
]
imuPub.publish(imuMsg)
if seq >= 300 and gyro_accuracy == 3 and cal_gyro == 1:
# Stop dynamically calibrating the gyro to avoid unwanted drift
rospy.loginfo("Stopping dynamic gyro calibration")
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] == 'b'):
self._battery_value = float(lineParts[1])
#######################################################################################################################
self._battery_pub.publish(self._battery_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._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()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self._frame_id
imu_msg.header = h
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)
pass
from math import atan;
from math import atan2;
from math import sqrt;
import tf
import os
rospy.init_node("node")
pub = rospy.Publisher('imu_data', Imu, queue_size=10)
north_pub = rospy.Publisher('north', std_msgs.msg.Float32, queue_size=10)
imuMsg = Imu()
imuMsg.orientation_covariance = [999999 , 0 , 0,
0, 9999999, 0,
0, 0, 999999]
imuMsg.angular_velocity_covariance = [9999, 0 , 0,
0 , 99999, 0,
0 , 0 , 0.02]
imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0,
0 , 0.2, 0,
0 , 0 , 0.2]
PI = 3.141592
grad2rad = PI/180.0
used_port='/dev/ttyACM0'
#find the last ttyACM*
#for possible_port in os.listdir('/dev/'):
# if possible_port.startswith("ttyACM"):
# used_port='/dev/'+possible_port
ax, ay, az = bno.read_linear_acceleration()
# print x, y, z, w, vx, vy, vz, ax, ay, az
quat = Quaternion(w, x, y, z)
quat_norm = quat.normalised
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = "odom"
imu_msg.orientation.w = w #quat_norm.elements[0]
imu_msg.orientation.z = z #quat_norm.elements[1]
imu_msg.orientation.y = y #quat_norm.elements[2]
imu_msg.orientation.x = x #quat_norm.elements[3]
imu_msg.orientation_covariance = [2, 0, 0, 0, 2, 0, 0, 0, 2]
imu_msg.angular_velocity.x = vx
imu_msg.angular_velocity.y = vy
imu_msg.angular_velocity.z = vz
imu_msg.angular_velocity_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1]
imu_msg.linear_acceleration.x = ax
imu_msg.linear_acceleration.y = ay
imu_msg.linear_acceleration.z = az
imu_msg.linear_acceleration_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1]
pub.publish(imu_msg)
# print('{0:0.2F} {1:0.2F} {2:0.2F} {3:0.2F} {4:0.2F} {5:0.2F} {6:0.2F} {7:0.2F} {8:0.2F} {9:0.2F}'.format(
# x,y,z,w,vx,vy,vz,ax,ay,az))
time.sleep(0.1)
def talker():
QUEUE_SIZE = 10
RATE_PUBLISHER = 50 # Frequency in Hz
SIZE_VECTOR = 3
MEAN_ACCEL = 0
MEAN_GYRO = 0
STD_DEVIATION_ACCEL = 0.2
STD_DEVIATION_GYRO = 0.1
INITIAL_YAW = 0
INITIAL_PITCH = 0
INITIAL_ROLL = 0
G = 9.81
# ROS Setup
pub = rospy.Publisher('python_IMU', Imu, queue_size = QUEUE_SIZE)
rospy.init_node('Python_IMU_Publisher_node')
rate = rospy.Rate(RATE_PUBLISHER)
while not rospy.is_shutdown():
print('Publishing new IMU Data')
yaw = INITIAL_YAW
pitch = INITIAL_PITCH
roll = INITIAL_ROLL
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]
# Noise is modelled as a gaussian distribution
noise_accel = numpy.random.normal(MEAN_ACCEL, STD_DEVIATION_ACCEL, SIZE_VECTOR)
noise_gyro = numpy.random.normal(MEAN_GYRO, STD_DEVIATION_GYRO, SIZE_VECTOR)
# ------------------------------------------ #
# Publish IMU data with added gaussian noise
# ------------------------------------------ #
# Accelerometer measurements
imuMsg.linear_acceleration.x = 0 + noise_accel[0]
imuMsg.linear_acceleration.y = 0 + noise_accel[1]
imuMsg.linear_acceleration.z = G + noise_accel[2]
# Gyroscope measurements
imuMsg.angular_velocity.x = 0 + noise_gyro[0]
imuMsg.angular_velocity.y = 0 + noise_gyro[1]
imuMsg.angular_velocity.z = 0 + noise_gyro[2]
# Quaternion
imuMsg.orientation.x = 0
imuMsg.orientation.y = 0
imuMsg.orientation.z = 0
imuMsg.orientation.w = 1
# Time and frame name
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'base_link'
# Publish and wait to achieve the correct frequency
pub.publish(imuMsg)
rate.sleep()
def compute_imu_msg(self):
# Create new message
try:
# Get data
self.get_imu_data()
[
q1, q2, q3, accuracy, roll, pitch, yaw, w_x, w_y, w_z, acc_x,
acc_y, acc_z
] = self.parse_msg()
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix + "imu"
imu_msg.header.stamp = rospy.Time.now() # + rospy.Duration(0.5)
if ((q1 * q1) + (q2 * q2) + (q3 * q3)) < 1:
q4 = [
q1, q2, q3,
np.sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)))
]
else:
self.log("Inconsistent readings from IMU", 2, alert="warn")
return True
# Compute the Orientation Quaternion
new_q = Quaternion()
new_q.x = q1
new_q.y = q2
new_q.z = q3
new_q.w = q4
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001
]
# Angular Velocity
imu_msg.angular_velocity.x = round(w_x, 4)
imu_msg.angular_velocity.y = round(w_y, 4)
imu_msg.angular_velocity.z = round(w_z, 4)
# Datasheet says:
# - Noise Spectral Density: 0.015dps/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(0.015/np.sqrt(20), 2)
# factor = 0.02
# imu_msg.angular_velocity_covariance = [
# diag, w_x*factor, w_x*factor,
# w_y*factor, diag, w_y*factor,
# w_z*factor, w_z*factor, diag
# ]
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = 0.0001
imu_msg.angular_velocity_covariance[4] = 0.0001
imu_msg.angular_velocity_covariance[8] = 0.0001
# imu_msg.angular_velocity_covariance = [-1] * 9
# Linear Acceleration
imu_msg.linear_acceleration.x = round(acc_x, 4)
imu_msg.linear_acceleration.y = round(acc_y, 4)
imu_msg.linear_acceleration.z = round(acc_z, 4)
# imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = 9.82
# imu_msg.linear_acceleration_covariance = [-1] * 9
# Datasheet says:
# - Noise Spectral Density: 230microg/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(230e-6/np.sqrt(20), 2)/256.
# factor = 0.02/256.
# imu_msg.linear_acceleration_covariance = [
# diag, acc_x*factor, acc_x*factor,
# acc_y*factor, diag, acc_y*factor,
# acc_z*factor, acc_z*factor, diag
# ]
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.001
imu_msg.linear_acceleration_covariance[4] = 0.001
imu_msg.linear_acceleration_covariance[8] = 0.001
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion(
[new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(
r, p, y))
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 _create_imu_msg(self,topic):
""" Create imu message from ROV data
Raises:
Exception: No data available
"""
# 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_topics[topic][3].publish(msg)
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('Vel')
position_data = data.get('Pos')
rawgps_data = data.get('RAWGPS')
status = data.get('Stat') # int
# 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:
x = imu_data['gyrX']
y = imu_data['gyrY']
z = imu_data['gyrZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.angular_velocity.x = v[0]
imu_msg.angular_velocity.y = v[1]
imu_msg.angular_velocity.z = v[2]
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 = v[0]
vel_msg.twist.angular.y = v[1]
vel_msg.twist.angular.z = v[2]
pub_vel = True
except KeyError:
pass
try:
x = imu_data['accX']
y = imu_data['accY']
z = imu_data['accZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.linear_acceleration.x = v[0]
imu_msg.linear_acceleration.y = v[1]
imu_msg.linear_acceleration.z = v[2]
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0.,
0.0004)
pub_imu = True
except KeyError:
pass
try:
x = imu_data['magX']
y = imu_data['magY']
z = imu_data['magZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
mag_msg.vector.x = v[0]
mag_msg.vector.y = v[1]
mag_msg.vector.z = v[2]
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)
def run(self):
"""Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
The IMU message, if fully filled in, contains information on orientation,
acceleration (in m/s^2), and angular rate (in radians/sec). For each of
these quantities, the IMU message format also wants the corresponding
covariance matrix.
Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
data entry is marked invalid. We do this by setting the first
entry of its associated covariance matrix to -1. The covariance
matrices are the 3x3 matrix with the axes' variance in the
diagonal. We obtain the variance from the Wiimote instance.
"""
rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
self.threadName = "IMU topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, canonicalNunchukAccel,
canonicalAngleRate) = self.obtainWiimoteData()
msg = Imu(
header=None,
orientation=None, # will default to [0.,0.,0.,0],
orientation_covariance=[
-1., 0., 0., 0., 0., 0., 0., 0., 0.
], # -1 indicates that orientation is unknown
angular_velocity=None,
angular_velocity_covariance=self.
angular_velocity_covariance,
linear_acceleration=None,
linear_acceleration_covariance=self.
linear_acceleration_covariance)
# If a gyro is plugged into the Wiimote, then note the
# angular velocity in the message, else indicate with
# the special gyroAbsence_covariance matrix that angular
# velocity is unavailable:
if self.wiistate.motionPlusPresent:
msg.angular_velocity.x = canonicalAngleRate[PHI]
msg.angular_velocity.y = canonicalAngleRate[THETA]
msg.angular_velocity.z = canonicalAngleRate[PSI]
else:
msg.angular_velocity_covariance = self.gyroAbsence_covariance
msg.linear_acceleration.x = canonicalAccel[X]
msg.linear_acceleration.y = canonicalAccel[Y]
msg.linear_acceleration.z = canonicalAccel[Z]
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
try:
self.pub.publish(msg)
except rospy.ROSException:
rospy.loginfo(
"Topic imu/data closed. Shutting down Imu sender.")
exit(0)
#rospy.logdebug("IMU state:")
#rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate))
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Imu sender.")
exit(0)
def pub_imu_callback(self, event):
""" This method is a callback of a timer. This publishes imu and pressure
sensor data """ # TODO: euler rate is not angular velocity!
# First publish pressure sensor
msg = PressureSensor()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'pressure_sensor'
msg.temperature = 25.0
msg.pressure = self.odom.pose.pose.position.z * self.water_density * 9.81 / 100000.0
self.pub_pressure.publish(msg)
# Imu
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = 'adis_imu'
# Add some noise
roll = self.orientation[0] + np.random.normal(
0.0, self.imu_orientation_covariance[0])
pitch = self.orientation[1] + np.random.normal(
0.0, self.imu_orientation_covariance[1])
yaw = self.orientation[2] + np.random.normal(
0.0, self.imu_orientation_covariance[2])
# Rotate as necessary
vehicle_rpy = PyKDL.Rotation.RPY(roll, pitch, yaw)
imu_orientation = self.imu_tf.M * vehicle_rpy
# Get RPY
angle = imu_orientation.GetRPY()
# Derive to obtain rates
if not self.imu_init:
# Initialize heading buffer in order to apply a
# savitzky_golay derivation
if len(self.heading_buffer) == 0:
self.heading_buffer.append(angle[2])
inc = cola2_lib.normalizeAngle(angle[2] - self.heading_buffer[-1])
self.heading_buffer.append(self.heading_buffer[-1] + inc)
if len(self.heading_buffer) == len(self.savitzky_golay_coeffs):
self.imu_init = True
# Euler derivation to roll and pitch, so:
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
else:
period = (imu.header.stamp - self.last_imu_update).to_sec()
if period < 0.001:
period = 0.001 # Minimum allowed period
# For yaw rate we apply a savitzky_golay derivation
inc = cola2_lib.normalizeAngle(angle[2] - self.heading_buffer[-1])
self.heading_buffer.append(self.heading_buffer[-1] + inc)
self.heading_buffer.pop(0)
imu.angular_velocity.z = np.convolve(self.heading_buffer,
self.savitzky_golay_coeffs,
mode='valid') / period
# TODO: Roll rate and Pitch rate should be also
# savitzky_golay derivations?
imu.angular_velocity.x = cola2_lib.normalizeAngle(
angle[0] - self.last_imu_orientation[0]) / period
imu.angular_velocity.y = cola2_lib.normalizeAngle(
angle[1] - self.last_imu_orientation[1]) / period
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
imu.angular_velocity_covariance = [
0.0001, 0., 0., 0., 0.0001, 0., 0., 0., 0.0002
]
# Euler to Quaternion
angle = tf.transformations.quaternion_from_euler(
angle[0], angle[1], angle[2])
imu.orientation.x = angle[0]
imu.orientation.y = angle[1]
imu.orientation.z = angle[2]
imu.orientation.w = angle[3]
imu.orientation_covariance[0] = self.imu_orientation_covariance[0]
imu.orientation_covariance[4] = self.imu_orientation_covariance[1]
imu.orientation_covariance[8] = self.imu_orientation_covariance[2]
#rospy.loginfo("%s: publishing imu", self.name)
self.pub_imu.publish(imu)
# Publish TF
imu_tf = tf.TransformBroadcaster()
o = tf.transformations.quaternion_from_euler(
math.radians(self.imu_tf_array[3]),
math.radians(self.imu_tf_array[4]),
math.radians(self.imu_tf_array[5]), 'sxyz')
imu_tf.sendTransform(
(self.imu_tf_array[0], self.imu_tf_array[1], self.imu_tf_array[2]),
o, imu.header.stamp, 'adis_imu_from_sim_nav_sensors', 'hug')
calib_data_print += line
rospy.loginfo(calib_data_print)
'''
roll = 0
pitch = 0
yaw = 0
seq = 0
rospy.loginfo("Giving the razor IMU board 3 seconds to boot...")
rospy.sleep(3) # Sleep for 5 seconds to wait for the board to boot
### configure board ###
# Orientation covariance estimation:
imuMsg.orientation_covariance = ORIENT_COVARIANCE
imuMsg.angular_velocity_covariance = ANGL_VEL_COVARIANCE
imuMsg.linear_acceleration_covariance = LIN_ACCEL_COVARIANCE
# ser.write(PAUSE_LOGGING)
ser.write(DISABLE_TIME)
ser.write(DISABLE_COMPASS)
ser.write(ENABLE_EULER)
ser.write(ENABLE_QUAT)
# Read a sample and print
print("Sample IMU Data:", ser.readline())
while not rospy.is_shutdown():
line = ser.readline()
# IMU data: <timeMS>, <accelX>, <accelY>, <accelZ>, <gyroX>, <gyroY>, <gyroZ>, <magX>, <magY>, <magZ>
# 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]
# read basic informations
port = rospy.get_param('~port', '/dev/ttyUSB0')
topic_name = rospy.get_param('~topic', 'imu')
link_name = rospy.get_param('~link_name', 'base_imu_link')
# accelerometer
accel_x_min = rospy.get_param('~accel_x_min', -250.0)
accel_x_max = rospy.get_param('~accel_x_max', 250.0)
def talker():
global con
pub_va = rospy.Publisher('imu/data_raw', Imu)
pub_m = rospy.Publisher('imu/mag', Vector3Stamped)
srv = rospy.Service('/tablet/command', StringSrv, handle_calls)
rospy.init_node('imu')
imu = Imu()
mag = Vector3Stamped()
last = ""
imu.header.frame_id = mag.header.frame_id = "/tablet"
v = math.pow(0.000028, 2)
imu.angular_velocity_covariance = [v, 0, 0, 0, v, 0, 0, 0, v]
imu.linear_acceleration_covariance = [v, 0, 0, 0, v, 0, 0, 0, v]
imu.orientation_covariance = [v, 0, 0, 0, v, 0, 0, 0, v]
while not rospy.is_shutdown():
s = last + con.read()
last = ""
up_imu = False
up_mag = False
for l in s.split("\n"):
if l[-1:] != '|':
last = l
continue
s = l[:-1].split(", ")
try:
for i in range(0, (len(s) - 1) / 4):
t = int(s[i * 4 + 1])
x = float(s[i * 4 + 2])
y = float(s[i * 4 + 3])
z = float(s[i * 4 + 4])
if t == 3:
imu.linear_acceleration.x = x
imu.linear_acceleration.y = y
imu.linear_acceleration.z = z
up_imu = True
elif t == 4:
imu.angular_velocity.x = x
imu.angular_velocity.y = y
imu.angular_velocity.z = z
up_imu = True
elif t == 5:
l = math.sqrt(x * x + y * y + z * z)
mag.vector.x = x / l
mag.vector.y = y / l
mag.vector.z = z / l
up_mag = True
elif t == 6:
qor = tf.transformations.quaternion_from_euler(x, y, z)
imu.orientation.x = qor[0]
imu.orientation.y = qor[1]
imu.orientation.z = qor[2]
imu.orientation.w = qor[3]
except Exception as e:
print e
if up_imu:
imu.header.stamp = rospy.Time.now()
pub_va.publish(imu)
if up_mag:
mag.header.stamp = rospy.Time.now()
pub_m.publish(mag)
con.close()
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
# if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
# print 'no fix'
# return
# Changing from NED from the Applanix to ENU in ROS
# Roll is still roll, since it's WRT to the x axis of the vehicle
# Pitch is -ve since axis goes the other way (+y to right vs left)
# Yaw (or heading) in Applanix is clockwise starting with North
# In ROS it's counterclockwise startin with East
time_stat = TimeSync()
time_stat.ros_time = rospy.Time.now()
time_stat.gps_time = data.td
self.pub_time.publish(time_stat)
t1 = time_stat.ros_time.secs + time_stat.ros_time.nsecs / 1E9
t2 = time_stat.gps_time.time1
# print '{0:6f}'.format(t1 - t2)
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch),
RAD(90 - data.heading)).GetQuaternion()
# UTM conversion
utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude)
# Initialize starting point if we haven't yet
if not self.init and self.zero_start:
self.origin.x = utm_pos.easting
self.origin.y = utm_pos.northing
self.origin.z = data.altitude
self.init = True
origin_param = {
"east": self.origin.x,
"north": self.origin.y,
"alt": self.origin.z,
}
rospy.set_param('/gps_origin', origin_param)
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
p.position.z = self.origin.z
self.pub_origin.publish(p)
#
# Odometry
# TODO: Work out these covariances properly from DOP
#
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = utm_pos.easting - self.origin.x
odom.pose.pose.position.y = utm_pos.northing - self.origin.y
odom.pose.pose.position.z = data.altitude - self.origin.z
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /reference frame or /vehicle frame and
# NED to ENU conversion is needed here too
odom.twist.twist.linear.x = data.east_vel
odom.twist.twist.linear.y = data.north_vel
odom.twist.twist.linear.z = -data.down_vel
odom.twist.twist.angular.x = RAD(data.ang_rate_long)
odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z
q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion(
*orient).z, Quaternion(*orient).w
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp,
odom.child_frame_id,
odom.header.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
navsat.latitude = data.latitude
navsat.longitude = data.longitude
navsat.altitude = data.altitude
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.pub_navsatfix.publish(navsat)
#
# IMU
# TODO: Work out these covariances properly
#
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
imu.orientation = Quaternion(*orient)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates
imu.angular_velocity.x = RAD(data.ang_rate_long)
imu.angular_velocity.y = RAD(-data.ang_rate_trans)
imu.angular_velocity.z = RAD(-data.ang_rate_down)
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration
imu.linear_acceleration.x = data.long_accel
imu.linear_acceleration.y = data.trans_accel
imu.linear_acceleration.z = data.down_accel
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
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._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] == 's'):
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 get_sensor_data(self):
"""Read IMU data from the sensor, parse and publish."""
# Initialize ROS msgs
imu_raw_msg = Imu()
imu_msg = Imu()
mag_msg = MagneticField()
temp_msg = Temperature()
# read from sensor
buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45)
# Publish raw data
imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg()
imu_raw_msg.header.frame_id = self.param.frame_id.value
# TODO: do headers need sequence counters now?
# imu_raw_msg.header.seq = seq
# TODO: make this an option to publish?
imu_raw_msg.orientation_covariance = [
self.param.variance_orientation.value[0], 0.0, 0.0, 0.0,
self.param.variance_orientation.value[1], 0.0, 0.0, 0.0,
self.param.variance_orientation.value[2]
]
imu_raw_msg.linear_acceleration.x = \
self.unpackBytesToFloat(buf[0], buf[1]) / self.param.acc_factor.value
imu_raw_msg.linear_acceleration.y = \
self.unpackBytesToFloat(buf[2], buf[3]) / self.param.acc_factor.value
imu_raw_msg.linear_acceleration.z = \
self.unpackBytesToFloat(buf[4], buf[5]) / self.param.acc_factor.value
imu_raw_msg.linear_acceleration_covariance = [
self.param.variance_acc.value[0], 0.0, 0.0, 0.0,
self.param.variance_acc.value[1], 0.0, 0.0, 0.0,
self.param.variance_acc.value[2]
]
imu_raw_msg.angular_velocity.x = \
self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value
imu_raw_msg.angular_velocity.y = \
self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value
imu_raw_msg.angular_velocity.z = \
self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value
imu_raw_msg.angular_velocity_covariance = [
self.param.variance_angular_vel.value[0], 0.0, 0.0, 0.0,
self.param.variance_angular_vel.value[1], 0.0, 0.0, 0.0,
self.param.variance_angular_vel.value[2]
]
# node.get_logger().info('Publishing imu message')
self.pub_imu_raw.publish(imu_raw_msg)
# TODO: make this an option to publish?
# Publish filtered data
imu_msg.header.stamp = self.node.get_clock().now().to_msg()
imu_msg.header.frame_id = self.param.frame_id.value
q = Quaternion()
# imu_msg.header.seq = seq
q.w = self.unpackBytesToFloat(buf[24], buf[25])
q.x = self.unpackBytesToFloat(buf[26], buf[27])
q.y = self.unpackBytesToFloat(buf[28], buf[29])
q.z = self.unpackBytesToFloat(buf[30], buf[31])
# TODO(flynneva): replace with standard normalize() function
# normalize
norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
imu_msg.orientation.x = q.x / norm
imu_msg.orientation.y = q.y / norm
imu_msg.orientation.z = q.z / norm
imu_msg.orientation.w = q.w / norm
imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance
imu_msg.linear_acceleration.x = \
self.unpackBytesToFloat(buf[32], buf[33]) / self.param.acc_factor.value
imu_msg.linear_acceleration.y = \
self.unpackBytesToFloat(buf[34], buf[35]) / self.param.acc_factor.value
imu_msg.linear_acceleration.z = \
self.unpackBytesToFloat(buf[36], buf[37]) / self.param.acc_factor.value
imu_msg.linear_acceleration_covariance = imu_raw_msg.linear_acceleration_covariance
imu_msg.angular_velocity.x = \
self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value
imu_msg.angular_velocity.y = \
self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value
imu_msg.angular_velocity.z = \
self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value
imu_msg.angular_velocity_covariance = imu_raw_msg.angular_velocity_covariance
self.pub_imu.publish(imu_msg)
# Publish magnetometer data
mag_msg.header.stamp = self.node.get_clock().now().to_msg()
mag_msg.header.frame_id = self.param.frame_id.value
# mag_msg.header.seq = seq
mag_msg.magnetic_field.x = \
self.unpackBytesToFloat(buf[6], buf[7]) / self.param.mag_factor.value
mag_msg.magnetic_field.y = \
self.unpackBytesToFloat(buf[8], buf[9]) / self.param.mag_factor.value
mag_msg.magnetic_field.z = \
self.unpackBytesToFloat(buf[10], buf[11]) / self.param.mag_factor.value
mag_msg.magnetic_field_covariance = [
self.param.variance_mag.value[0], 0.0, 0.0, 0.0,
self.param.variance_mag.value[1], 0.0, 0.0, 0.0,
self.param.variance_mag.value[2]
]
self.pub_mag.publish(mag_msg)
# Publish temperature
temp_msg.header.stamp = self.node.get_clock().now().to_msg()
temp_msg.header.frame_id = self.param.frame_id.value
# temp_msg.header.seq = seq
temp_msg.temperature = float(buf[44])
self.pub_temp.publish(temp_msg)
