def publish(self, event):
compass_accel = self.compass_accel.read()
compass = compass_accel[0:3]
accel = compass_accel[3:6]
gyro = self.gyro.read()
# Put together an IMU message
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD
imu_msg.linear_acceleration.x = accel[0] * G
imu_msg.linear_acceleration.y = accel[1] * G
imu_msg.linear_acceleration.z = accel[2] * G
self.pub_imu.publish(imu_msg)
# Put together a magnetometer message
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = compass[0]
mag_msg.magnetic_field.y = compass[1]
mag_msg.magnetic_field.z = compass[2]
self.pub_mag.publish(mag_msg)
def publish(self, event):
compass_accel = self.compass_accel.read()
compass = compass_accel[0:3]
accel = compass_accel[3:6]
gyro = self.gyro.read()
# Put together an IMU message
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD
imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD
imu_msg.linear_acceleration.x = accel[0] * G
imu_msg.linear_acceleration.y = accel[1] * G
imu_msg.linear_acceleration.z = accel[2] * G
self.pub_imu.publish(imu_msg)
# Put together a magnetometer message
mag_msg = MagneticField()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.magnetic_field.x = compass[0]
mag_msg.magnetic_field.y = compass[1]
mag_msg.magnetic_field.z = compass[2]
self.pub_mag.publish(mag_msg)
def read_from_imu(self):
while True:
try:
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'imu'
orientation = self.bno.read_quaternion()
linear_acceleration = self.bno.read_linear_acceleration()
angular_velocity = self.bno.read_gyroscope()
imu_msg.orientation_covariance[0] = -1
imu_msg.linear_acceleration_covariance[0] = -1
imu_msg.angular_velocity_covariance[0] = -1
imu_msg.orientation = Quaternion(orientation[1],
orientation[2],
orientation[3],
orientation[0])
imu_msg.linear_acceleration = Vector3(linear_acceleration[0],
linear_acceleration[1],
linear_acceleration[2])
imu_msg.angular_velocity = Vector3(np.deg2rad(angular_velocity[0]),
np.deg2rad(angular_velocity[1]),
np.deg2rad(angular_velocity[2]))
return imu_msg
except IOError:
pass
def fill_imu_msg(self, accel_gs, omega_dps):
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'imu'
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity = Vector3(omega_dps[0] * pi / 180.0,
omega_dps[1] * pi / 180.0,
omega_dps[2] * pi / 180.0)
imu_msg.linear_acceleration = Vector3(accel_gs[0] * self._GRAVITY,
accel_gs[1] * self._GRAVITY,
accel_gs[2] * self._GRAVITY)
t2 = rospy.Time.now()
t1 = self.prev_time
self.prev_time = t2
dt = (t2 - t1).to_sec()
self.heading += omega_dps[2] * pi / 180.0 * dt
if (self.heading > 3.14159):
self.heading += -6.28318
elif (self.heading <= -3.14159):
self.heading += 6.28318
heading_msg = Float32()
heading_msg.data = self.heading * 180.0 / 3.14159
return imu_msg, heading_msg
def send_imu_msgs(self):
"""
send imu messages with respect to ego_vehicle frame
:return:
"""
vehicle_acc_wrt_world_carla = self.carla_actor.get_acceleration()
vehicle_ang_vel_wrt_world_carla = self.carla_actor.get_angular_velocity(
)
vehicle_orientation_wrt_world_carla = self.carla_actor.get_transform(
).rotation
vehicle_orientation_wrt_world_ros = transforms.carla_rotation_to_ros_quaternion(
vehicle_orientation_wrt_world_carla)
tmp_twist = transforms.carla_velocity_to_ros_twist(
vehicle_acc_wrt_world_carla, vehicle_ang_vel_wrt_world_carla,
vehicle_orientation_wrt_world_carla)
vehicle_acc_wrt_body_ros, vehicle_ang_vel_wrt_body_ros = tmp_twist.linear, tmp_twist.angular
imu_info = Imu(header=self.get_msg_header())
imu_info.header.frame_id = self.get_prefix()
imu_info.orientation = vehicle_orientation_wrt_world_ros
imu_info.angular_velocity = vehicle_ang_vel_wrt_body_ros
imu_info.linear_acceleration = vehicle_acc_wrt_body_ros
imu_info.linear_acceleration.z += 9.8
self.publish_message(self.get_topic_prefix() + "/imu", imu_info)
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 publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def imu_publisher():
global accl_x, accl_y, accl_z, gyro_x, gyro_y, gyro_z, comp_x, comp_y, comp_z
rospy.init_node(
'Imu_mpu9250_node', anonymous=False
) # if anonymous=True is to ensure that node is unique by adding random number in the end
r = rospy.Rate(20.0)
current_time = rospy.Time.now()
curn_time = time.time()
last_time = time.time()
new_time = 0.0
while not rospy.is_shutdown():
current_time = rospy.Time.now()
read_imu_raw_data()
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.angular_velocity = Vector3(gyro_x, gyro_y, gyro_z)
imu.linear_acceleration = Vector3(accl_x, accl_y, accl_z)
imu_data_pub.publish(imu)
mag = MagneticField()
mag.header.stamp = current_time
mag.header.frame_id = "imu/mag"
mag.magnetic_field = Vector3(comp_x, comp_y, comp_z)
imu_mag_pub.publish(mag)
r.sleep()
def imu():
i2c = board.I2C() # uses board.SCL and board.SDA
sensor = LSM6DS33(i2c)
rospy.loginfo("Initializing imu publisher")
imu_pub = rospy.Publisher('/imu', Imu, queue_size=5)
rospy.loginfo("Publishing Imu at: " + imu_pub.resolved_name)
rospy.init_node('imu_node')
rate = rospy.Rate(10) # 50hz
while not rospy.is_shutdown():
rospy.loginfo("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" %
(sensor.acceleration))
rospy.loginfo("Gyro X:%.2f, Y: %.2f, Z: %.2f radians/s" %
(sensor.gyro))
rospy.loginfo("")
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = 'LSM6DS33 6-DoF IMU'
imu.orientation_covariance[0] = -1
imu.angular_velocity = create_vector3(sensor.gyro[0], sensor.gyro[1],
sensor.gyro[2])
imu.linear_acceleration_covariance[0] = -1
imu.linear_acceleration = create_vector3(sensor.acceleration[0],
sensor.acceleration[1],
sensor.acceleration[2])
imu.angular_velocity_covariance[0] = -1
imu_pub.publish(imu)
rate.sleep()
def init_values(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
i = Imu()
i.header.stamp = rospy.Time.now()
i.header.frame_id = 'Riptide_INS'
i.orientation = create_quaternion(0, 0, 0, 0)
i.angular_velocity = create_vector3(1, 2, 3)
self.imu_pub.publish(i)
gpsmsg = NavSatFix()
gpsmsg.header.stamp = rospy.Time.now()
gpsmsg.header.frame_id = "gps"
gpsmsg.latitude = float(0) # /!\ change here
gpsmsg.longitude = float(0) # /!\ change here
self.gps_pub.publish(gpsmsg)
depth = Float32()
depth.data = 1 # /!\ change here
self.depth_pub.publish(Float32(data=1))
rate.sleep()
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def publish_imu(self, pose, twist):
msg = Imu()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu_link'
msg.orientation = pose.orientation
msg.angular_velocity = twist.angular
self.imu_pub.publish(msg)
def imu_msg_callback(msg, cb_args):
pub = cb_args[0]
global last_lin_acc
global last_ang_vel
send = Imu()
send.header = msg.header
send.header.stamp = rospy.Time.now()
# send.header.frame_id = "camera_imu_frame"
# if msg.linear_acceleration_covariance[0] != -1.0:
# last_lin_acc.x = msg.linear_acceleration.z
# last_lin_acc.y = msg.linear_acceleration.x * -1
# last_lin_acc.z = msg.linear_acceleration.y * -1
# elif msg.angular_velocity_covariance[0] != -1.0:
# last_ang_vel.x = msg.angular_velocity.z
# last_ang_vel.y = msg.angular_velocity.x * -1
# last_ang_vel.z = msg.angular_velocity.y * -1
if msg.linear_acceleration_covariance[0] != -1.0:
last_lin_acc = msg.linear_acceleration
elif msg.angular_velocity_covariance[0] != -1.0:
last_ang_vel = msg.angular_velocity
send.angular_velocity = last_ang_vel
send.linear_acceleration = last_lin_acc
send.orientation_covariance[0] = -1.0
pub.publish(send)
def imuResponseCallback(self, msg):
#############################################################################
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.orientation = msg.orientation
imu_msg.angular_velocity = msg.angular_velocity
imu_msg.linear_acceleration = msg.linear_acceleration
imu_msg.orientation_covariance[0] = msg.orientation_covariance[0]
imu_msg.orientation_covariance[4] = msg.orientation_covariance[4]
if self.rtkEnabled == 0:
imu_msg.orientation_covariance[8] = msg.orientation_covariance[8]
else:
imu_msg.orientation_covariance[8] = msg.orientation_covariance[
8] + 99999 / (1 + math.exp(-8 * (abs(self.totalVel) - 2))
) # sigmoid function to mix yaw from the IMU
imu_msg.angular_velocity_covariance[
0] = msg.angular_velocity_covariance[0]
imu_msg.angular_velocity_covariance[
4] = msg.angular_velocity_covariance[4]
imu_msg.angular_velocity_covariance[
8] = msg.angular_velocity_covariance[8]
imu_msg.linear_acceleration_covariance[
0] = msg.linear_acceleration_covariance[0]
imu_msg.linear_acceleration_covariance[
4] = msg.linear_acceleration_covariance[4]
imu_msg.linear_acceleration_covariance[
8] = msg.linear_acceleration_covariance[8]
#rospy.logerr("Vel: %f Z Rot Covariance: %f", self.totalVel, imu_msg.orientation_covariance[8])
self.imuPub.publish(imu_msg)
def CreateBag(args): #img,imu, bagname, timestamps
'''read time stamps'''
imutimesteps, imudata = ReadGT(args[1]) #the url of IMU data
if not os.path.exists(args[2]):
os.system(r'touch %s' % args[2])
bag = rosbag.Bag(args[2], 'w')
try:
for i in range(len(imudata)):
imu = Imu()
angular_v = Vector3()
linear_a = Vector3()
angular_v.x = float(imudata[i][3])
angular_v.y = float(imudata[i][4])
angular_v.z = float(imudata[i][5])
linear_a.x = float(imudata[i][0])
linear_a.y = float(imudata[i][1])
linear_a.z = float(imudata[i][2])
imuStamp = rospy.rostime.Time.from_sec(float(imutimesteps[i]))
imu.header.stamp = imuStamp
imu.angular_velocity = angular_v
imu.linear_acceleration = linear_a
bag.write("IMU/imu_raw", imu, imuStamp)
finally:
bag.close()
def talker():
imu_data = Imu()
odom = Odometry()
imu_pub = rospy.Publisher('Imu', Imu, queue_size=1)
odom_pub = rospy.Publisher('Odom', Odometry, queue_size=1)
rospy.init_node('LISTEN_ODOM_IMU')
rate = rospy.Rate(100)
while not rospy.is_shutdown():
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = 'IMU (BASE)'
imu_data.header = h
q, g, a, twist_linear, twist_angular, pose_pt = serial_listen()
imu_data.orientation = q
imu_data.angular_velocity = g
imu_data.linear_acceleration = a
covariance = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
imu_data.orientation_covariance = covariance
imu_data.angular_velocity_covariance = covariance
imu_data.linear_acceleration_covariance = covariance
odom.twist.twist.linear = twist_linear
odom.twist.twist.angular = twist_angular
odom.pose.pose.position = pose_pt
imu_pub.publish(imu_data)
odom_pub.publish(odom)
rospy.loginfo('PUBLISHED...')
rate.sleep()
def publish_sensor_data(self, sensor_data):
acc = np.array(sensor_data.acceleration.data) / 1000.0 * g
ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi
# ang_vel = rotate_v(ang_vel.tolist(), self.rotation)
msg = Imu()
msg.header.frame_id = self.base_frame_id
msg.header.stamp = rospy.Time.now()
msg.linear_acceleration = Vector3(*acc)
# It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame
q = sensor_data.quaternion.data
q = [q[1], q[2], q[3], q[0]]
msg.orientation = Quaternion(*q)
# msg.orientation = Quaternion(*self.orientation)
msg.angular_velocity = Vector3(*ang_vel)
msg.linear_acceleration_covariance = self.linear_acceleration_cov
msg.orientation_covariance = self.orientation_cov
msg.angular_velocity_covariance = self.angular_velocity_cov
self.imu_pub.publish(msg)
mag = np.array(sensor_data.magnetic.data) * 1e-6
m_msg = MagneticField()
m_msg.header = msg.header
m_msg.magnetic_field = Vector3(*mag)
m_msg.magnetic_field_covariance = self.magnetic_field_cov
self.mag_pub.publish(m_msg)
def publish_sensor_data(self):
# IMU FEEDBACK
imu = Imu()
vec1 = Vector3(-self.received_imu[2][0], self.received_imu[1][0],
self.received_imu[0][0])
imu.angular_velocity = vec1
vec2 = Vector3(self.received_imu[5][0], self.received_imu[4][0],
self.received_imu[3][0])
imu.linear_acceleration = vec2
self.pub.publish(imu)
# MOTOR FEEDBACK
# Convert motor array from the embedded coordinate system to that
# used by controls
ctrlAngleArray = mcuToCtrlAngles(self.received_angles)
robotState = RobotState()
for i in range(12):
robotState.joint_angles[i] = ctrlAngleArray[i][0]
# Convert motor array from the embedded order and sign convention
# to that used by controls
m = getCtrlToMcuAngleMap()
robotState.joint_angles[0:12] = np.linalg.inv(m).dot(
robotState.joint_angles[0:18])[0:12]
self.pub2.publish(robotState)
def parse_imu(self, data):
'''
Given data from jaguar imu, parse and return a standard IMU message.
Return None when given bad data, or no complete message was found in data.
'''
# Use regular expressions to extract complete message
# message format: $val,val,val,val,val,val,val,val,val,val#\n
hit = re.search(r"\$-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*#", data)
if not hit:
# if there are no hits, return None
return None
else:
imu_data = hit.group(0)
try:
# Try to format the data
imu_data = imu_data[1:-1].split(",")
#Format (from drrobot docs): seq, accelx, accely, accelz, gyroY, gyroZ, gyroX, magnetonX, magnetonY, magnetonZ
seq = int(imu_data[0])
accelx = float(imu_data[1])
accely = float(imu_data[2])
accelz = float(imu_data[3])
gyroy = float(imu_data[4])
gyroz = float(imu_data[5])
gyrox = float(imu_data[6])
magnetonx = float(imu_data[7])
magnetony = float(imu_data[8])
magnetonz = float(imu_data[9])
rot_mat = [ [float(imu_data[10]), float(imu_data[11]), float(imu_data[12])],
[float(imu_data[13]), float(imu_data[14]), float(imu_data[15])],
[float(imu_data[16]), float(imu_data[17]), float(imu_data[18])] ]
except:
# bad data in match, pass
return None
else:
# data formatted fine, build message and publish
# if we didn't get a magnetometer update, set to current reading
if magnetonz == 0:
magnetonx = self.current_mag[0]
magnetony = self.current_mag[1]
magnetonz = self.current_mag[2]
# otherwise, update current magnetometer
else:
self.current_mag = [magnetonx, magnetony, magnetonz]
# Calculate quaternion from given rotation matrix
quat = rot_mat_to_quat(rot_mat);
# Build message
msg = Imu()
msg.header = Header(stamp = rospy.Time.now(), frame_id = "imu")
msg.linear_acceleration = Vector3(accelx, accely, accelz)
msg.angular_velocity = Vector3(gyrox, gyroy, gyroz)
if quat != None: msg.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
return msg
def publishImuData(self):
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.linear_acceleration = self.Acc
imu_msg.orientation = self.normQuat
imu_msg.angular_velocity = self.Gyro
self.imuPub.publish(imu_msg)
def fill_imu_msg(self, accel, mag):
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'imu'
imu_msg.orientation_covariance[0] = -1
imu_msg.angular_velocity = Vector3(mag[0], mag[1], mag[2])
imu_msg.linear_acceleration = Vector3(-accel[1] / 100, accel[0] / 100,
accel[2] / 100)
return imu_msg
def publish_imu_data(self):
imu_msg = Imu()
imu_msg.orientation = self.__get_quaternions()
# imu_msg.orientation_covariance=
imu_msg.angular_velocity = self.__get_angular_velocity()
# imu_msg.angular_velocity_covariance=
imu_msg.linear_acceleration = self.__get_linear_acceleration()
# imu_msg.linear_acceleration_covariance=
return imu_msg
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 fill_imu_msg(self, accel, omega): imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'imu' imu_msg.orientation_covariance[0] = -1 imu_msg.angular_velocity = Vector3(omega[0], omega[1], omega[2]) imu_msg.linear_acceleration = Vector3(accel[0]*self._GRAVITY , accel[1]*self._GRAVITY, accel[2]*self._GRAVITY) #rospy.loginfo(imu_msg.linear_acceleration) return imu_msg
def publish_imu_data (imu_data) :
mag_msg = Vector3Stamped()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.vector = Vector3(float(imu_data[0]),float(imu_data[1]),float(imu_data[2]))
mag_pub.publish(mag_msg)
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.linear_acceleration = Vector3(float(imu_data[3]),float(imu_data[4]),float(imu_data[5]))
imu_msg.angular_velocity = Vector3(float(imu_data[6]),float(imu_data[7]),float(imu_data[8]))
imu_pub.publish(imu_msg)
def imuCallback(data):
imuData = Imu()
imuData.header.stamp = rospy.Time.now()
imuData.header.frame_id = 'imu_link'
imuData.orientation = data.orientation
imuData.orientation_covariance = data.orientation_covariance
imuData.angular_velocity = data.angular_velocity
imuData.angular_velocity_covariance = data.angular_velocity_covariance
imuData.linear_acceleration = data.linear_acceleration
imuData.linear_acceleration_covariance = data.linear_acceleration_covariance
pub.publish(imuData)
def publish_imu_data (imu_data) :
mag_msg = Vector3Stamped()
mag_msg.header.stamp = rospy.Time.now()
mag_msg.vector = Vector3(float(imu_data[0]),float(imu_data[1]),float(imu_data[2]))
mag_pub.publish(mag_msg)
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.linear_acceleration = Vector3(float(imu_data[3]),float(imu_data[4]),float(imu_data[5]))
imu_msg.angular_velocity = Vector3(float(imu_data[6]),float(imu_data[7]),float(imu_data[8]))
imu_pub.publish(imu_msg)
def imu_raw_callback(self, msg):
if self.start_vo:
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.angular_velocity = msg.angular_velocity
imu_msg.angular_velocity_covariance = msg.angular_velocity_covariance
imu_msg.linear_acceleration = msg.linear_acceleration
imu_msg.linear_acceleration_covariance = msg.linear_acceleration_covariance
imu_msg.header.frame_id = "imu"
for i in range(0, 9, 4):
imu_msg.orientation_covariance[i] = IMU_SIGMA**2
self.imupub.publish(imu_msg)
def callbackImu(msg):
# Grab accelerometer and gyro data.
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = 'imu_link'
q = np.array([msg.quaternion.x, msg.quaternion.y, msg.quaternion.z, msg.quaternion.w])
q = normalize(q)
euler = tf.transformations.euler_from_quaternion(q)
q = tf.transformations.quaternion_from_euler(-(euler[0] + math.pi / 2), euler[1], euler[2] - math.pi)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
euler_new = tf.transformations.euler_from_quaternion(q)
rospy.loginfo("euler[0] = %s, euler[1] = %s, euler = [2] = %s",
str(euler_new[0]), str(euler_new[1]), str(euler_new[2]))
imu_msg.orientation_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
imu_msg.angular_velocity = msg.gyro
imu_msg.angular_velocity_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
# Estimate gyro bias.
global gyro_readings
global MAX_GYRO_READINGS
global gyro_bias
if len(gyro_readings) < MAX_GYRO_READINGS:
gyro_readings.append(imu_msg.angular_velocity.z)
elif math.isnan(gyro_bias):
gyro_bias = sum(gyro_readings)/MAX_GYRO_READINGS
if not math.isnan(gyro_bias):
imu_msg.angular_velocity.z -= gyro_bias
imu_msg.linear_acceleration = msg.accelerometer
imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \
0, 0.90, 0, \
0, 0, 0.90]
# Grab magnetometer data.
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.magnetic_field = msg.magnetometer
# TODO(gbrooks): Add covariance.
# Publish sensor data.
imu_pub.publish(imu_msg)
mag_pub.publish(mag_msg)
def publish_sensor_data(self, received_angles, received_imu):
# IMU FEEDBACK
imu = Imu()
imu.header.stamp = rp.rostime.get_rostime()
imu.header.frame_id = "imu_link"
# TODO autocalibrate
imu.angular_velocity = Vector3(
(-received_imu[2][0] - self._imu_calibration["gyro_offset"][0]) *
self._imu_calibration["gryo_scale"][0],
(received_imu[1][0] - self._imu_calibration["gyro_offset"][1]) *
self._imu_calibration["gryo_scale"][1],
(received_imu[0][0] - self._imu_calibration["gyro_offset"][2]) *
self._imu_calibration["gryo_scale"][2])
imu.linear_acceleration = Vector3(
(received_imu[5][0] - self._imu_calibration["acc_offset"][0]) *
self._imu_calibration["acc_scale"][0],
(received_imu[4][0] - self._imu_calibration["acc_offset"][1]) *
self._imu_calibration["acc_scale"][1],
(received_imu[3][0] - self._imu_calibration["acc_offset"][2]) *
self._imu_calibration["acc_scale"][2])
imu.orientation_covariance[0] = -1
self._pub_imu.publish(imu)
# MOTOR FEEDBACK
joint_state = JointState()
joint_state.header.stamp = rp.rostime.get_rostime()
for motor in self._motor_map:
if int(self._motor_map[motor]["id"]) < 12:
assert (int(self._motor_map[motor]["id"]) <
len(received_angles))
angle = received_angles[int(self._motor_map[motor]["id"])]
if math.isnan(angle): # TODO fix this
continue
angle = (angle - float(self._motor_map[motor]["offset"])
) * float(self._motor_map[motor]["direction"])
angle = np.deg2rad(angle)
else:
angle = self._motor_map[motor]["value"]
# Joint controller state
state = JointControllerState()
state.process_value = angle
state.command = self._motor_map[motor]["value"]
state.error = angle - self._motor_map[motor]["value"]
state.process_value_dot = 0 # TODO PID settings and process value dot
state.header.stamp = rp.rostime.get_rostime()
self._motor_map[motor]["publisher"].publish(state)
# Joint State
joint_state.name.append(motor)
joint_state.position.append(angle)
self._pub_joint_states.publish(joint_state)
def _publish_data(self):
acc = self.read_accel()
gyro = self.read_gyro()
msg = Imu()
msg.header.frame_id = self.frame_id
msg.orientation_covariance = [
-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
] # indicate orientation unknown
msg.linear_acceleration = Vector3(acc[0], acc[1], acc[2])
msg.angular_velocity = Vector3(gyro[0], gyro[1], gyro[2])
rospy.loginfo("Gyro: %s linear: %s" % (acc, gyro))
self.pub.publish(msg)
def talker():
pub = rospy.Publisher('Imu', Imu, queue_size=10)
rospy.init_node('talker')
rate = rospy.Rate(50)
msg = Imu()
for i in range(9):
msg.orientation_covariance[i] = -1
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
# rad_to_deg = 180/pi
#getting linear_accleration
#converts G's to m/s^2 as per ros spec
accel = mpu9250.readAccel()
ax = (accel['x']) / 9.8
ay = (accel['y']) / 9.8
az = (accel['z']) / 9.8
#getting angular velocity
# gyro is set to deg/s to rad/s as per ros spec
gyro = mpu9250.readGyro()
gx = (gyro['x']) * deg_to_rad
gy = (gyro['y']) * deg_to_rad
gz = (gyro['z']) * deg_to_rad
#getting quaternion
'''
x = math.sin(gy)*math.sin(gz)*math.cos(gx)+math.cos(gy)*math.cos(gz)*math.sin(gx)
y = math.sin(gy)*math.cos(gz)*math.cos(gx)+math.cos(gy)*math.sin(gz)*math.sin(gx)
z = math.cos(gy)*math.sin(gz)*math.cos(gx)-math.sin(gy)*math.cos(gz)*math.sin(gx)
w = math.cos(gy)*math.cos(gz)*math.cos(gx)-math.sin(gy)*math.sin(gz)*math.sin(gx)
'''
x = 0
y = 0
z = 0
w = 0
#setting av
msg.angular_velocity = Vector3(gx, gy, gz)
#setting la
msg.linear_acceleration = Vector3(ax, ay, az)
#setting quate
msg.orientation = Quaternion(x, y, z, w)
rospy.loginfo(msg)
pub.publish(msg)
rate.sleep()
def msg_process(self, msg):
msg_topic = msg.topic.split("/")
if (msg_topic[-1] == "imu"):
topic_name = msg_topic[0].replace(" ", "_")
msg_list = msg.payload.split(";")
pkgSizeAcc = int(msg_list[0])
moduleSizeAcc = 4 * pkgSizeAcc + 2
acc_msg = msg_list[2:moduleSizeAcc]
pkgSizeGyro = int(msg_list[moduleSizeAcc])
moduleSizeGyro = 4 * pkgSizeGyro + 2
gyro_msg = msg_list[moduleSizeAcc + 2:moduleSizeAcc +
moduleSizeGyro]
biggerpkg = max([pkgSizeAcc, pkgSizeGyro])
acceleration = Vector3()
velocity = Vector3()
for i in range(0, biggerpkg):
time = ""
if i < pkgSizeGyro:
velocity = Vector3()
velocity.x = float(gyro_msg[3 * i].replace(',', '.'))
velocity.y = float(gyro_msg[3 * i + 1].replace(',', '.'))
velocity.z = float(gyro_msg[3 * i + 2].replace(',', '.'))
time = gyro_msg[3 * pkgSizeGyro + i]
if i < pkgSizeAcc:
acceleration = Vector3()
acceleration.x = float(acc_msg[3 * i].replace(',', '.'))
acceleration.y = float(acc_msg[3 * i + 1].replace(
',', '.'))
acceleration.z = float(acc_msg[3 * i + 2].replace(
',', '.'))
time = acc_msg[3 * pkgSizeAcc + i]
imu_message = Imu()
now = rospy.get_rostime()
imu_message.header.stamp.secs = now.secs
imu_message.header.stamp.nsecs = now.nsecs
imu_message.header.frame_id = time
imu_message.angular_velocity = velocity
imu_message.linear_acceleration = acceleration
imu_publisher = rospy.Publisher(topic_name, Imu, queue_size=1)
imu_publisher.publish(imu_message)
else:
print msg.topic + " is not a supported topic"
def callback(raw_data, pub):
imu = Imu()
imu.header = raw_data.header
# set imu.orientation quaternion
# Btw, robot_pose_ekf expects the Roll and Pitch angles to be
# absolute, but the Yaw angle to be relative. Compute the relative
# angle from the currently reported and previously reported absolute
# angle
r = raw_data.roll
p = raw_data.pitch
y = raw_data.yaw - callback.old_yaw
callback.old_yaw = raw_data.yaw
quat = quaternion_from_euler(r, p, y, "sxyz")
imu.orientation.x = quat[0]
imu.orientation.y = quat[1]
imu.orientation.z = quat[2]
imu.orientation.w = quat[3]
#TODO: set imu.orientation_covariance
#row major about x, y, z
# According to OS5000 datasheet, accuracy
# depends on tilt:
# 0.5 deg RMS level heading,
# 1 deg typical RMS accuracy < +-30deg tilt,
# 1.5deg < +-60deg tilt
# Roll and Pitch:
# Typical 1deg accuracy for tilt < +-30 deg
# Assume tilt < +- 30 deg, set all standard
# deviations to 1 degree
std_dev = from_degrees(1)
imu.orientation_covariance[0] = std_dev**2
imu.orientation_covariance[4] = std_dev**2
# standard deviation on yaw is doubled since
# it's computed as a difference of two measurements,
# each with a std_dev of 1 degree
imu.orientation_covariance[8] = (2 * std_dev)**2
# no angular velocity data is available, so set
# element 0 of covariance matrix to -1
# alternatively, set the covariance really high
imu.angular_velocity = Vector3(0, 0, 0)
imu.angular_velocity_covariance[0] = -1
# Set linear acceleration
imu.linear_acceleration = raw_data.acceleration
# TODO: Set linear acceleration covariance
imu.linear_acceleration_covariance[0] = -1
pub.publish(imu)
def callback(raw_data, pub):
imu = Imu()
imu.header = raw_data.header
# set imu.orientation quaternion
# Btw, robot_pose_ekf expects the Roll and Pitch angles to be
# absolute, but the Yaw angle to be relative. Compute the relative
# angle from the currently reported and previously reported absolute
# angle
r = raw_data.roll
p = raw_data.pitch
y = raw_data.yaw - callback.old_yaw
callback.old_yaw = raw_data.yaw
quat = quaternion_from_euler(r,p,y,"sxyz")
imu.orientation.x = quat[0]
imu.orientation.y = quat[1]
imu.orientation.z = quat[2]
imu.orientation.w = quat[3]
#TODO: set imu.orientation_covariance
#row major about x, y, z
# According to OS5000 datasheet, accuracy
# depends on tilt:
# 0.5 deg RMS level heading,
# 1 deg typical RMS accuracy < +-30deg tilt,
# 1.5deg < +-60deg tilt
# Roll and Pitch:
# Typical 1deg accuracy for tilt < +-30 deg
# Assume tilt < +- 30 deg, set all standard
# deviations to 1 degree
std_dev = from_degrees(1)
imu.orientation_covariance[0] = std_dev**2
imu.orientation_covariance[4] = std_dev**2
# standard deviation on yaw is doubled since
# it's computed as a difference of two measurements,
# each with a std_dev of 1 degree
imu.orientation_covariance[8] = (2*std_dev)**2
# no angular velocity data is available, so set
# element 0 of covariance matrix to -1
# alternatively, set the covariance really high
imu.angular_velocity = Vector3(0,0,0)
imu.angular_velocity_covariance[0] = -1
# Set linear acceleration
imu.linear_acceleration = raw_data.acceleration
# TODO: Set linear acceleration covariance
imu.linear_acceleration_covariance[0] = -1
pub.publish(imu)
def convert_to_baselink(self, imu_msg, imu_model, transform):
# convert imu_msg from its frame to baselink frame
# Assumption! TODO: I'm going to assume that the axis are aligned between frames to start
# come back to this later, and rotate to align axis first
# proposed path:
# -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link
# -> run the same code below
'''
[sensor_msgs/Imu]:
std_msgs/Header header - DONE
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration - DONE
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance - DONE
'''
new_msg = Imu()
# Header
new_msg.header = imu_msg.header
new_msg.header.frame_id = '/base_link'
# Orientation (same based on Assumption! above)
new_msg.orientation = imu_msg.orientation
# including covariance, because same. will likely drop for rotation
new_msg.orientation_covariance = imu_msg.orientation_covariance
# Angular Velocity (same based on Assumption! above)
new_msg.angular_velocity = imu_msg.angular_velocity
# including covariance, because same. will likely drop for rotation
new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance
# Linear Acceleration (not the same, even with assumption)
new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform)
return new_msg
def run(self):
pub = rospy.Publisher('/um7', Imu, queue_size=10)
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
self.UM7(self.um7_serial)
mesg = Imu()
mesg.header.stamp = rospy.Time.now()
mesg.header.frame_id = 'UM7'
mesg.orientation = self.q
mesg.angular_velocity = Vector3(*self.velocities)
pub.publish(mesg)
rate.sleep()
def publishImuData(self):
imu_msg = Imu()
imu_msg.header.stamp = self.frameTime
#imu_msg.linear_acceleration = self.Acc
imu_msg.linear_acceleration = self.AccCart
if self.ref_mode:
imu_msg.orientation = self.refQuat
else:
imu_msg.orientation = self.Quat
imu_msg.angular_velocity = self.Acc
#print imu_msg
self.imuPub.publish(imu_msg)
def map_imu(values):
"""
Map the values generated with the hypothesis-ros imu strategy to a rospy imu type.
"""
if not isinstance(values, _Imu):
raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
ros_imu = Imu()
ros_imu.header = values.header
ros_imu.orientation = values.orientation
ros_imu.orientation_covariance = values.orientation_covariance
ros_imu.angular_velocity = values.angular_velocity
ros_imu.angular_velocity_covariance = values.angular_velocity_covariance
ros_imu.linear_acceleration = values.linear_acceleration
ros_imu.linear_acceleration_covariance = values.linear_acceleration_covariance
return ros_imu
def publish(self, data):
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = data.getOrientation()
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def imu_cb(self, imu_msg):
new_imu_msg = Imu()
new_imu_msg.header = imu_msg.header
new_imu_msg.orientation = imu_msg.orientation
new_imu_msg.orientation_covariance = imu_msg.orientation_covariance
new_imu_msg.angular_velocity = imu_msg.angular_velocity
new_imu_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance
new_imu_msg.linear_acceleration = imu_msg.linear_acceleration
new_imu_msg.linear_acceleration_covariance = imu_msg.linear_acceleration_covariance
self.imu_pub.publish(new_imu_msg)
def imuDataReceived(data):
global imu_repub
angles = tf.transformations.euler_from_quaternion(
[data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w]
)
imu_msg = Imu()
yaw = wrapToPi(angles[2] - math.pi / 2)
orientation = tf.transformations.quaternion_from_euler(0, 0, yaw)
imu_msg.orientation.x = orientation[0]
imu_msg.orientation.y = orientation[1]
imu_msg.orientation.z = orientation[2]
imu_msg.orientation.w = orientation[3]
imu_msg.header = data.header
imu_msg.orientation_covariance = data.orientation_covariance
imu_msg.angular_velocity = data.angular_velocity
imu_repub.publish(imu_msg)
def 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 talker():
"""Initializes the publisher node"""
global pub
pub = rospy.Publisher("mpu6050", Imu)
rospy.init_node("MPU6050-Driver")
# calibrate(imu, accel+gyro, samples = 0)
global seq
seq = 0
while not rospy.is_shutdown():
sample = Imu()
sample.header = make_header()
sample.orientation_covariance[0] = -1
sample.angular_velocity_covariance = [0] * 9
sample.angular_velocity = read_gyros()
sample.linear_acceleration_covariance = [0] * 9
sample.linear_acceleration = read_accels()
rospy.loginfo(str(sample))
pub.publish(sample)
time.sleep(0.1)
