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 rawimu_handler(self, rawimus):
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self.base_frame
# Populate orientation field with one from inspvax message.
imu.orientation = Quaternion(*self.orientation)
imu.orientation_covariance = self.orientation_covariance
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = rawimus.x_gyro * 0.1 / (3600.0 *
256.0) * self.imu_rate
imu.angular_velocity.y = -rawimus.my_gyro * 0.1 / (
3600.0 * 256.0) * self.imu_rate
imu.angular_velocity.z = rawimus.z_gyro * 0.1 / (3600.0 *
256.0) * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = rawimus.x_accel * 0.05 / 2**15 * self.imu_rate
imu.linear_acceleration.y = rawimus.my_accel * 0.05 / 2**15 * self.imu_rate
imu.linear_acceleration.z = rawimus.z_accel * 0.05 / 2**15 * self.imu_rate
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
def 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 corrimudata_handler(self, corrimudata):
# TODO: Work out these covariances properly. Logs provide covariances in local frame, not body
imu = Imu()
imu.header.stamp = rospy.Time.now()
# imu.header.frame_id = self.base_frame
imu.header.frame_id = 'imu_link'
# Populate orientation field with one from inspvax message.
imu.orientation = Quaternion(*self.orientation)
imu.orientation_covariance = self.orientation_covariance
# Angular rates (rad/s)
# corrimudata log provides instantaneous rates so multiply by IMU rate in Hz
imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate
imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate
imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration (m/s^2)
imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate
imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate
imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate + 9.7803
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
def 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 convert_imu(imu):
ret = Imu()
ret.header = convert_utime(imu.utime)
ret.orientation = rpy_to_quat(*imu.tb_angles)
ret.orientation_covariance = [0]*9
gyro = imu.gyro
w = ret.angular_velocity
w.x, w.y, w.z = gyro
# 5 dps tolerance at 0 with 3 percent linear tolerance
# 2 percent cross axis sensitivity
rps = 5/180*math.pi
cross = 0.02**2
ret.angular_velocity_covariance = [
(rps + gyro[0]*.03)**2, cross*(gyro[0]*gyro[1]), cross*(gyro[0]*gyro[2]),
cross*(gyro[0]*gyro[1]), (rps + gyro[1]*.03)**2, cross*(gyro[1]*gyro[2]),
cross*(gyro[0]*gyro[2]), cross*(gyro[1]*gyro[2]), (rps + gyro[2]*.03)**2
]
accel = imu.accel
a = ret.linear_acceleration
a.x, a.y, a.z = accel
# 3 percent sensitivity with 2 percent cross axis sensitivity
ret.linear_acceleration_covariance = [
(accel[0]*0.03)**2, cross*(accel[0]*accel[1]), cross*(accel[0]*accel[2]),
cross*(accel[0]*accel[1]), (accel[1]*0.03)**2, cross*(accel[1]*accel[2]),
cross*(accel[0]*accel[2]), cross*(accel[1]*accel[2]), (accel[2]*0.03)**2
]
return ret
def imu_publisher():
global accl_x, accl_y,accl_z, gyro_x, gyro_y, gyro_z, comp_x, comp_y, comp_z, quad_x, quad_y, quad_z, quad_w;
rospy.init_node('Imu_bno_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
rospy.loginfo(" Streaming IMU Data ..")
while not rospy.is_shutdown():
current_time = rospy.Time.now()
read_imu_raw_data()
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.orientation = Quaternion(quad_x, quad_y, quad_z, quad_w)
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 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 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 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 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 odom_subscriber_cb(data):
imu_msg = Imu()
imu_msg.header = data.header
imu_msg.header.frame_id = 'base_link'
imu_msg.orientation = data.pose.pose.orientation
imu_msg.linear_acceleration.z = -10
pub.publish(imu_msg)
def publish(self, theta, omega):
msg = Imu()
msg.header = self.header
msg.angular_velocity.x = omega[0]*1
msg.angular_velocity.y = omega[1]*1
msg.angular_velocity.z = omega[2]*1
msg.orientation = self.thetaToOrientation(theta)
grav = np.array([0,0,self.gravity], dtype=np.float64)
r = self.thetaToRotation(theta)
grav = r.apply(grav, True)
pureAcceleration = self.accel - grav
msg.linear_acceleration.x = pureAcceleration[0]*1
msg.linear_acceleration.y = pureAcceleration[1]*1
msg.linear_acceleration.z = pureAcceleration[2]*1
cov = np.zeros(9, dtype=np.float64)
msg.angular_velocity_covariance = cov
msg.orientation_covariance = cov
msg.linear_acceleration_covariance = cov
self.publisher.publish(msg)
def 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 on_imu(channel, data):
m = ins_t.decode(data)
pyTime = rospy.Time.from_sec(m.utime * 1E-6)
#print "Received ins message", pyTime
pose = PoseStamped()
t = rospy.get_rostime()
pose.header.stamp = pyTime
pose.header.frame_id = 'imu_link'
pose.pose.orientation.w = m.quat[0]
pose.pose.orientation.x = m.quat[1]
pose.pose.orientation.y = m.quat[2]
pose.pose.orientation.z = m.quat[3]
imuRosPosePub.publish(pose)
imu = Imu()
imu.header = pose.header
imu.orientation = pose.pose.orientation
imu.angular_velocity.x = m.gyro[0]
imu.angular_velocity.y = m.gyro[1]
imu.angular_velocity.z = m.gyro[2]
imu.linear_acceleration.x = m.accel[0]
imu.linear_acceleration.y = m.accel[1]
imu.linear_acceleration.z = m.accel[2]
imuRosPub.publish(imu)
def publish_imu(pub, ego_vehicle, sensor_file):
if not hasattr(publish_imu, 'imu_rot'):
with open(sensor_file) as f:
json_sensors = json.loads(f.read())
for sensor_spec in json_sensors['sensors']:
if 'imu' not in sensor_spec['type']:
continue
temp = np.array([sensor_spec['roll'], -sensor_spec['pitch'], -sensor_spec['yaw']])
publish_imu.imu_rot = Rotation.from_euler('xyz', temp, degrees=True)
carla_accel = ego_vehicle.get_acceleration()
carla_omega = ego_vehicle.get_angular_velocity()
carla_rot = ego_vehicle.get_transform().rotation
msg = Imu()
msg.header.frame_id = "tamagawa/imu_link"
msg.header.stamp = increment_clock.time
r_body = Rotation.from_euler('xyz', transforms.carla_rotation_to_RPY(carla_rot), degrees=False)
r = r_body * publish_imu.imu_rot
imu_accel = r.inv().apply(transforms.carla_velocity_to_numpy_vector(carla_accel)) # func wants vel, but its the same for accel
imu_omega = r.inv().apply(np.deg2rad(np.array([carla_omega.x, -carla_omega.y, -carla_omega.z])))
msg.linear_acceleration.x = imu_accel[0]
msg.linear_acceleration.y = imu_accel[1]
msg.linear_acceleration.z = imu_accel[2] - 9.8 # they want gravity subtracted
msg.angular_velocity.x = imu_omega[0]
msg.angular_velocity.y = imu_omega[1]
msg.angular_velocity.z = imu_omega[2]
re = r.as_euler('xyz', degrees=False)
quat = tf.transformations.quaternion_from_euler(re[0], re[1], re[2])
ros_quaternion = transforms.numpy_quaternion_to_ros_quaternion(quat)
msg.orientation = ros_quaternion
pub.publish(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 post_imu(self, component_instance):
""" Publish the data of the IMU sensor as a custom ROS Imu message
"""
parent = component_instance.robot_parent
parent_name = parent.blender_obj.name
current_time = rospy.Time.now()
imu = Imu()
imu.header.seq = self._seq
imu.header.stamp = current_time
imu.header.frame_id = "/imu"
imu.orientation = self.orientation.to_quaternion()
imu.angular_velocity.x = component_instance.local_data['angular_velocity'][0]
imu.angular_velocity.y = component_instance.local_data['angular_velocity'][1]
imu.angular_velocity.z = component_instance.local_data['angular_velocity'][2]
imu.linear_acceleration.x = component_instance.local_data['linear_acceleration'][0]
imu.linear_acceleration.y = component_instance.local_data['linear_acceleration'][1]
imu.linear_acceleration.z = component_instance.local_data['linear_acceleration'][2]
for topic in self._topics:
# publish the message on the correct w
if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name):
topic.publish(imu)
self._seq += 1
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(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 sensor_data_updated(self, carla_imu_measurement):
"""
Function to transform a received imu measurement into a ROS Imu message
:param carla_imu_measurement: carla imu measurement object
:type carla_imu_measurement: carla.IMUMeasurement
"""
imu_msg = Imu()
imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp)
imu_msg.angular_velocity.x = carla_imu_measurement.gyroscope.x
imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y
imu_msg.angular_velocity.z = carla_imu_measurement.gyroscope.z
imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x
imu_msg.linear_acceleration.y = carla_imu_measurement.accelerometer.y
imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z
imu_rotation = carla_imu_measurement.transform.rotation
quat = tf.transformations.quaternion_from_euler(math.radians(imu_rotation.roll),
math.radians(imu_rotation.pitch),
math.radians(imu_rotation.yaw))
imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat)
self.publish_message(
self.get_topic_prefix(), imu_msg)
def 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 sensor_data_updated(self, carla_imu_measurement):
"""
Function to transform a received imu measurement into a ROS Imu message
:param carla_imu_measurement: carla imu measurement object
:type carla_imu_measurement: carla.IMUMeasurement
"""
imu_msg = Imu()
imu_msg.header = self.get_msg_header(timestamp=carla_imu_measurement.timestamp)
# Carla uses a left-handed coordinate convention (X forward, Y right, Z up).
# Here, these measurements are converted to the right-handed ROS convention
# (X forward, Y left, Z up).
imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x
imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y
imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z
imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x
imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y
imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z
imu_rotation = carla_imu_measurement.transform.rotation
quat = trans.carla_rotation_to_numpy_quaternion(imu_rotation)
imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat)
self.publish_message(
self.get_topic_prefix(), imu_msg)
def 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 publish_marker(self, e):
# create IMU message and publish
imu_msg = Imu()
imu_msg.header.stamp = rospy.get_rostime()
imu_msg.header.frame_id = "imu"
imu_msg.orientation = self.pose.orientation
self.imu_publisher.publish(imu_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 pub_imu(broadcast):
global theta
imu_msg = Imu()
imu_msg.header.stamp = rospy.get_rostime()
robot_id = rospy.get_namespace().replace('/', '')
imu_msg.header.frame_id = '{}_yaw_earth'.format(robot_id) #it contain only yaw
quaternion = Quaternion(*tf.transformations.quaternion_from_euler(0,0,theta))
imu_msg.orientation = quaternion
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 angles(self, data):
orientation_and_pose = Imu()
orientation_and_pose.orientation = data.orientation
orientation_list = [
data.orientation.x, data.orientation.y, data.orientation.z,
data.orientation.w
]
(R, P, Y) = euler_from_quaternion(orientation_list)
self.RPY[0] = degrees(R)
self.RPY[1] = degrees(P)
self.RPY[2] = degrees(Y)
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 __update__(self, imu:Imu):
'''
Update routine, to be called by subscribers.
Args:
imu [Imu]: The ROS message 'sensor_msgs.msg.Imu'.
'''
o = imu.orientation
source = R.from_quat([o.x, o.y, o.z, o.w])
remapped = np.matmul(self.target.as_matrix(), source.as_matrix())
imu.orientation = Quaternion(*R.from_matrix(remapped).as_quat())
self.pub.publish(imu)
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 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 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 default(self, ci='unused'):
imu = Imu()
imu.header = self.get_ros_header()
imu.orientation = self.component_instance.bge_object.worldOrientation.to_quaternion()
imu.angular_velocity.x = self.data['angular_velocity'][0]
imu.angular_velocity.y = self.data['angular_velocity'][1]
imu.angular_velocity.z = self.data['angular_velocity'][2]
imu.linear_acceleration.x = self.data['linear_acceleration'][0]
imu.linear_acceleration.y = self.data['linear_acceleration'][1]
imu.linear_acceleration.z = self.data['linear_acceleration'][2]
self.publish(imu)
def map_imu(values):
"""
Map the values generated with the hypothesis-ros imu strategy to a rospy imu type.
"""
if not isinstance(values, _Imu):
raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
ros_imu = Imu()
ros_imu.header = values.header
ros_imu.orientation = values.orientation
ros_imu.orientation_covariance = values.orientation_covariance
ros_imu.angular_velocity = values.angular_velocity
ros_imu.angular_velocity_covariance = values.angular_velocity_covariance
ros_imu.linear_acceleration = values.linear_acceleration
ros_imu.linear_acceleration_covariance = values.linear_acceleration_covariance
return ros_imu
def publishImuMsg(self):
#This code do not consider covariance
tmp = Imu()
tmp.header.frame_id = "base_link"
tmp.orientation = self.orientation
tmp.linear_acceleration.x = self.twist.linear.x - self.last_values[0]
tmp.linear_acceleration.y = self.twist.linear.y - self.last_values[1]
tmp.angular_velocity.z = self.twist.angular.z - self.last_values[2]
self.imu_Publisher.publish(tmp)
self.last_values = [
tmp.linear_acceleration.x, tmp.linear_acceleration.y,
tmp.angular_velocity.z
]
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 default(self, ci='unused'):
""" Publish the data of the IMU sensor as a custom ROS Imu message """
imu = Imu()
imu.header = self.get_ros_header()
imu.orientation = self.orientation
imu.angular_velocity.x = self.data['angular_velocity'][0]
imu.angular_velocity.y = self.data['angular_velocity'][1]
imu.angular_velocity.z = self.data['angular_velocity'][2]
imu.linear_acceleration.x = self.data['linear_acceleration'][0]
imu.linear_acceleration.y = self.data['linear_acceleration'][1]
imu.linear_acceleration.z = self.data['linear_acceleration'][2]
self.publish(imu)
def 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 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 = rospy.Time.now()
imu_msg.header.frame_id = 'base_link'
#Orientation from Accelerometer
roll = self.accel_data["roll"]
pitch = self.accel_data["pitch"]
yaw = 0
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
imu_msg.orientation = Quaternion(*q)
#Angular Velocity from Gyroscope
imu_msg.angular_velocity.x = self.gyro_data["x"]
imu_msg.angular_velocity.y = self.gyro_data["y"]
imu_msg.angular_velocity.z = self.gyro_data["z"]
#Linear Acceleration from Accelerometer
imu_msg.linear_acceleration.x = self.accel_data["x"]
imu_msg.linear_acceleration.y = self.accel_data["y"]
imu_msg.linear_acceleration.z = self.accel_data["z"]
self.imu_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 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 run(self):
while not rospy.is_shutdown():
# Get and publish all the sensors data
for sensor, id_number in SENSORS.items():
imu_msg = Imu()
mag_msg = Vector3Stamped()
# Order: GyroRate[0:2], AccelerometerVector[3:5], CompassVector[6:8]
#~ raw_data = self.pvr_system.getAllCorrectedComponentSensorData(id_number)
tared_quat = self.pvr_system.getTaredOrientationAsQuaternion(id_number)
#~ if raw_data and tared_quat:
if tared_quat:
imu_msg.orientation = Quaternion(*tared_quat)
#~ imu_msg.angular_velocity = Vector3(*raw_data[0:3])
#~ imu_msg.linear_acceleration = Vector3(*raw_data[3:6])
#~ mag_msg.vector = Vector3(*raw_data[6:9])
# Publish the data
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = 'world'
mag_msg.header = imu_msg.header
self.mag_pub[sensor].publish(mag_msg)
self.imu_pub[sensor].publish(imu_msg)
def magnetic_cb(msg):
global x_max, x_min, y_max, y_min, x_center, y_center, x_scale, y_scale
if autocompute:
x_max = max(x_max, msg.vector.x)
x_min = min(x_min, msg.vector.x)
y_max = max(y_max, msg.vector.y)
y_min = min(y_min, msg.vector.y)
x_center = (x_max + x_min) / 2.0
y_center = (y_max + y_min) / 2.0
x_scale = x_max - x_min
y_scale = y_max - y_min
x = (msg.vector.x - x_center) / x_scale
y = (msg.vector.y - y_center) / y_scale
heading = atan2(x, y)
imu = Imu()
imu.header = msg.header
imu.header.frame_id = 'odom'
q = tf.transformations.quaternion_from_euler(0, 0, heading)
imu.orientation = Quaternion(*q)
imu.orientation_covariance = [ compass_var, 0.0, 0.0,
0.0, compass_var, 0.0,
0.0, 0.0, compass_var ]
imu.angular_velocity_covariance = [ -1, 0, 0,
0, 0, 0,
0, 0, 0 ]
imu.linear_acceleration_covariance = [ -1, 0, 0,
0, 0, 0,
0, 0, 0 ]
imu_pub.publish(imu)
def scan_matcher_callback(self, data):
self.current_time = rospy.Time.now()
dt = (self.current_time - self.last_time).to_sec()
# Calculate Relative Yaw
if self.init_imu_angle:
self.last_imu_angle = data.theta
self.last_time = self.current_time
self.init_imu_angle = False
return
current_imu_angle = data.theta
dyaw = 0.0
if (-3.1416 <= self.last_imu_angle < 0) and (0 < current_imu_angle <= 3.1416):
dyaw = -((-3.1416 - self.last_imu_angle) + (3.1416 - current_imu_angle))
elif (0 <= self.last_imu_angle < 3.1416) and (-3.1416 < current_imu_angle < 0):
dyaw = ((3.1416- self.last_imu_angle) + (-3.1416 - current_imu_angle))
else:
dyaw = current_imu_angle - self.last_imu_angle
# inverse direction to match imu_link
# dyaw = -dyaw
imu_msg = Imu()
imu_msg.header.frame_id = "imu_link"
imu_msg.header.stamp = rospy.Time.now()
q_tmp = tf.transformations.quaternion_from_euler(0, 0, data.theta)
imu_msg.orientation = Quaternion(q_tmp[0], q_tmp[1], q_tmp[2], q_tmp[3])
imu_msg.angular_velocity.x = 0
imu_msg.angular_velocity.y = 0
if dt == 0.0:
imu_msg.angular_velocity.z = 0.0
else:
imu_msg.angular_velocity.z = dyaw/dt
imu_noises = pow(0.00017, 2) # = 0.01 degrees / sec
imu_msg.orientation_covariance = [1e3, 0, 0, 0, 1e3, 0, 0, 0, imu_noises]
self.last_imu_angle = current_imu_angle
self.last_time = self.current_time
self.imu_pub.publish(imu_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)
def update_sensors(self):
# print "accelerometer:", self._bridge.get_accelerometer()
# print "proximity:", self._bridge.get_proximity()
# print "light:", self._bridge.get_light_sensor()
# print "motor_position:", self._bridge.get_motor_position()
# print "floor:", self._bridge.get_floor_sensors()
# print "image:", self._bridge.get_image()
## If image from camera
if self.enabled_sensors['camera']:
# Get Image
image = self._bridge.get_image()
#print image
if image is not None:
nimage = np.asarray(image)
image_msg = CvBridge().cv2_to_imgmsg(nimage, "rgb8")
self.image_publisher.publish(image_msg)
if self.enabled_sensors['proximity']:
prox_sensors = self._bridge.get_proximity()
for i in range(0,8):
if prox_sensors[i] > 0:
self.prox_msg[i].range = 0.5/math.sqrt(prox_sensors[i]) # Transform the analog value to a distance value in meters (given from field tests).
else:
self.prox_msg[i].range = self.prox_msg[i].max_range
if self.prox_msg[i].range > self.prox_msg[i].max_range:
self.prox_msg[i].range = self.prox_msg[i].max_range
if self.prox_msg[i].range < self.prox_msg[i].min_range:
self.prox_msg[i].range = self.prox_msg[i].min_range
self.prox_msg[i].header.stamp = rospy.Time.now()
self.prox_publisher[i].publish(self.prox_msg[i])
# e-puck proximity positions (cm), x pointing forward, y pointing left
# P7(3.5, 1.0) P0(3.5, -1.0)
# P6(2.5, 2.5) P1(2.5, -2.5)
# P5(0.0, 3.0) P2(0.0, -3.0)
# P4(-3.5, 2.0) P3(-3.5, -2.0)
#
# e-puck proximity orentations (degrees)
# P7(10) P0(350)
# P6(40) P1(320)
# P5(90) P2(270)
# P4(160) P3(200)
self.br.sendTransform((0.035, -0.010, 0.034), tf.transformations.quaternion_from_euler(0, 0, 6.11), rospy.Time.now(), self._name+"/base_prox0", self._name+"/base_link")
self.br.sendTransform((0.025, -0.025, 0.034), tf.transformations.quaternion_from_euler(0, 0, 5.59), rospy.Time.now(), self._name+"/base_prox1", self._name+"/base_link")
self.br.sendTransform((0.000, -0.030, 0.034), tf.transformations.quaternion_from_euler(0, 0, 4.71), rospy.Time.now(), self._name+"/base_prox2", self._name+"/base_link")
self.br.sendTransform((-0.035, -0.020, 0.034), tf.transformations.quaternion_from_euler(0, 0, 3.49), rospy.Time.now(), self._name+"/base_prox3", self._name+"/base_link")
self.br.sendTransform((-0.035, 0.020, 0.034), tf.transformations.quaternion_from_euler(0, 0, 2.8), rospy.Time.now(), self._name+"/base_prox4", self._name+"/base_link")
self.br.sendTransform((0.000, 0.030, 0.034), tf.transformations.quaternion_from_euler(0, 0, 1.57), rospy.Time.now(), self._name+"/base_prox5", self._name+"/base_link")
self.br.sendTransform((0.025, 0.025, 0.034), tf.transformations.quaternion_from_euler(0, 0, 0.70), rospy.Time.now(), self._name+"/base_prox6", self._name+"/base_link")
self.br.sendTransform((0.035, 0.010, 0.034), tf.transformations.quaternion_from_euler(0, 0, 0.17), rospy.Time.now(), self._name+"/base_prox7", self._name+"/base_link")
if self.enabled_sensors['accelerometer']:
accel = self._bridge.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)
if self.enabled_sensors['motor_position']:
motor_pos = list(self._bridge.get_motor_position()) # Get a list since tuple elements returned by the function are immutable.
# Convert to 16 bits signed integer.
if(motor_pos[0] & 0x8000):
motor_pos[0] = -0x10000 + motor_pos[0]
if(motor_pos[1] & 0x8000):
motor_pos[1] = -0x10000 + motor_pos[1]
#print "motor_pos: " + str(motor_pos[0]) + ", " + str(motor_pos[1])
self.leftStepsDiff = motor_pos[0]*MOT_STEP_DIST - self.leftStepsPrev # Expressed in meters.
self.rightStepsDiff = motor_pos[1]*MOT_STEP_DIST - self.rightStepsPrev # Expressed in meters.
#print "left, right steps diff: " + str(self.leftStepsDiff) + ", " + str(self.rightStepsDiff)
self.deltaTheta = (self.rightStepsDiff - self.leftStepsDiff)/WHEEL_DISTANCE # Expressed in radiant.
self.deltaSteps = (self.rightStepsDiff + self.leftStepsDiff)/2 # Expressed in meters.
#print "delta theta, steps: " + str(self.deltaTheta) + ", " + str(self.deltaSteps)
self.x_pos += self.deltaSteps*math.cos(self.theta + self.deltaTheta/2) # Expressed in meters.
self.y_pos += self.deltaSteps*math.sin(self.theta + self.deltaTheta/2) # Expressed in meters.
self.theta += self.deltaTheta # Expressed in radiant.
#print "x, y, theta: " + str(self.x_pos) + ", " + str(self.y_pos) + ", " + str(self.theta)
self.leftStepsPrev = motor_pos[0]*MOT_STEP_DIST # Expressed in meters.
self.rightStepsPrev = motor_pos[1]*MOT_STEP_DIST # Expressed in meters.
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = self._name+"/base_link"
odom_msg.pose.pose.position = Point(self.x_pos, self.y_pos, 0)
q = tf.transformations.quaternion_from_euler(0, 0, self.theta)
odom_msg.pose.pose.orientation = Quaternion(*q)
self.endTime = time.time()
odom_msg.twist.twist.linear.x = self.deltaSteps / (self.endTime-self.startTime) # self.deltaSteps is the linear distance covered in meters from the last update (delta distance);
# the time from the last update is measured in seconds thus to get m/s we multiply them.
odom_msg.twist.twist.angular.z = self.deltaTheta / (self.endTime-self.startTime) # self.deltaTheta is the angular distance covered in radiant from the last update (delta angle);
# the time from the last update is measured in seconds thus to get rad/s we multiply them.
#print "time elapsed = " + str(self.endTime-self.startTime) + " seconds"
self.startTime = self.endTime
self.odom_publisher.publish(odom_msg)
pos = (odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z)
ori = (odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z, odom_msg.pose.pose.orientation.w)
self.br.sendTransform(pos, ori, odom_msg.header.stamp, odom_msg.child_frame_id, odom_msg.header.frame_id)
if self.enabled_sensors['light']:
light_sensors = self._bridge.get_light_sensor()
light_sensors_marker_msg = Marker()
light_sensors_marker_msg.header.frame_id = self._name+"/base_link"
light_sensors_marker_msg.header.stamp = rospy.Time.now()
light_sensors_marker_msg.type = Marker.TEXT_VIEW_FACING
light_sensors_marker_msg.pose.position.x = 0.15
light_sensors_marker_msg.pose.position.y = 0
light_sensors_marker_msg.pose.position.z = 0.15
q = tf.transformations.quaternion_from_euler(0, 0, 0)
light_sensors_marker_msg.pose.orientation = Quaternion(*q)
light_sensors_marker_msg.scale.z = 0.01
light_sensors_marker_msg.color.a = 1.0
light_sensors_marker_msg.color.r = 1.0
light_sensors_marker_msg.color.g = 1.0
light_sensors_marker_msg.color.b = 1.0
light_sensors_marker_msg.text = "light: " + str(light_sensors)
self.light_publisher.publish(light_sensors_marker_msg)
if self.enabled_sensors['floor']:
floor_sensors = self._bridge.get_floor_sensors()
floor_marker_msg = Marker()
floor_marker_msg.header.frame_id = self._name+"/base_link"
floor_marker_msg.header.stamp = rospy.Time.now()
floor_marker_msg.type = Marker.TEXT_VIEW_FACING
floor_marker_msg.pose.position.x = 0.15
floor_marker_msg.pose.position.y = 0
floor_marker_msg.pose.position.z = 0.13
q = tf.transformations.quaternion_from_euler(0, 0, 0)
floor_marker_msg.pose.orientation = Quaternion(*q)
floor_marker_msg.scale.z = 0.01
floor_marker_msg.color.a = 1.0
floor_marker_msg.color.r = 1.0
floor_marker_msg.color.g = 1.0
floor_marker_msg.color.b = 1.0
floor_marker_msg.text = "floor: " + str(floor_sensors)
self.floor_publisher.publish(floor_marker_msg)
if self.enabled_sensors['selector']:
curr_sel = self._bridge.get_selector()
selector_marker_msg = Marker()
selector_marker_msg.header.frame_id = self._name+"/base_link"
selector_marker_msg.header.stamp = rospy.Time.now()
selector_marker_msg.type = Marker.TEXT_VIEW_FACING
selector_marker_msg.pose.position.x = 0.15
selector_marker_msg.pose.position.y = 0
selector_marker_msg.pose.position.z = 0.11
q = tf.transformations.quaternion_from_euler(0, 0, 0)
selector_marker_msg.pose.orientation = Quaternion(*q)
selector_marker_msg.scale.z = 0.01
selector_marker_msg.color.a = 1.0
selector_marker_msg.color.r = 1.0
selector_marker_msg.color.g = 1.0
selector_marker_msg.color.b = 1.0
selector_marker_msg.text = "selector: " + str(curr_sel)
self.selector_publisher.publish(selector_marker_msg)
if self.enabled_sensors['motor_speed']:
motor_speed = list(self._bridge.get_motor_speed()) # Get a list since tuple elements returned by the function are immutable.
# Convert to 16 bits signed integer.
if(motor_speed[0] & 0x8000):
motor_speed[0] = -0x10000 + motor_speed[0]
if(motor_speed[1] & 0x8000):
motor_speed[1] = -0x10000 + motor_speed[1]
motor_speed_marker_msg = Marker()
motor_speed_marker_msg.header.frame_id = self._name+"/base_link"
motor_speed_marker_msg.header.stamp = rospy.Time.now()
motor_speed_marker_msg.type = Marker.TEXT_VIEW_FACING
motor_speed_marker_msg.pose.position.x = 0.15
motor_speed_marker_msg.pose.position.y = 0
motor_speed_marker_msg.pose.position.z = 0.09
q = tf.transformations.quaternion_from_euler(0, 0, 0)
motor_speed_marker_msg.pose.orientation = Quaternion(*q)
motor_speed_marker_msg.scale.z = 0.01
motor_speed_marker_msg.color.a = 1.0
motor_speed_marker_msg.color.r = 1.0
motor_speed_marker_msg.color.g = 1.0
motor_speed_marker_msg.color.b = 1.0
motor_speed_marker_msg.text = "speed: " + str(motor_speed)
self.motor_speed_publisher.publish(motor_speed_marker_msg)
if self.enabled_sensors['microphone']:
mic = self._bridge.get_microphone()
microphone_marker_msg = Marker()
microphone_marker_msg.header.frame_id = self._name+"/base_link"
microphone_marker_msg.header.stamp = rospy.Time.now()
microphone_marker_msg.type = Marker.TEXT_VIEW_FACING
microphone_marker_msg.pose.position.x = 0.15
microphone_marker_msg.pose.position.y = 0
microphone_marker_msg.pose.position.z = 0.07
q = tf.transformations.quaternion_from_euler(0, 0, 0)
microphone_marker_msg.pose.orientation = Quaternion(*q)
microphone_marker_msg.scale.z = 0.01
microphone_marker_msg.color.a = 1.0
microphone_marker_msg.color.r = 1.0
microphone_marker_msg.color.g = 1.0
microphone_marker_msg.color.b = 1.0
microphone_marker_msg.text = "microphone: " + str(mic)
self.microphone_publisher.publish(microphone_marker_msg)
import imu_ukf
if __name__ == "__main__":
rospy.init_node("imu_sim")
omega = zeros((3,1))
state_pub = rospy.Publisher('state_sim', State)
ground_truth_pub = rospy.Publisher('imu_ground_truth', Imu)
gravity_vector = array([0.04,0.,9.8])
magnetic_vector = array([0.03,0.12,0.42])
yaw = 0.0
r = rospy.Rate(20)
while not rospy.is_shutdown():
yaw += 0.01
i = Imu()
i.header.stamp = rospy.Time.now()
i.orientation =\
Quaternion(*tf_math.quaternion_from_euler(0,0,yaw,'sxyz'))
i.angular_velocity.z = 0.01
s = State()
s.header.stamp = rospy.Time.now()
s.magnetometer.x = magnetic_vector[0]
s.magnetometer.y = magnetic_vector[1]
s.magnetometer.z = magnetic_vector[2]
s.linear_acceleration.x = gravity_vector[0]
s.linear_acceleration.y = gravity_vector[1]
s.linear_acceleration.z = gravity_vector[2]
s.angular_velocity.z = 0.01
state_pub.publish(s)
ground_truth_pub.publish(i)
r.sleep()
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Quaternion
if __name__ == "__main__":
try:
rospy.init_node('imu_zero')
# publish (0,0,0,1)
pub_imu_zero = rospy.Publisher("/imu/zero", Imu, queue_size=10)
rate = rospy.Rate(10)
zero = Imu()
zero.header.frame_id = "odom"
zero.orientation = Quaternion(0, 0, 0, 1)
while not rospy.is_shutdown():
pub_imu_zero.publish(zero)
rate.sleep()
except rospy.ROSInterruptException:
pass
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:
return
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion()
# UTM conversion
(zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude)
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
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 = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.altitude
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /vehicle frame
odom.twist.twist.linear.x = data.speed
odom.twist.twist.linear.y = 0
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)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.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.y = 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
# angular velocity
imu_data.angular_velocity.x = Gx
imu_data.angular_velocity.y = Gy
imu_data.angular_velocity.z = Gz
# acceleration
imu_data.linear_acceleration.x = Ax
imu_data.linear_acceleration.y = Ay
imu_data.linear_acceleration.z = Az
# temperature
temp.data = T;
# Quaternion message dataaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa
Imu_data.orientation = Quaternion()
# orientation
#ai = imu_data.orientation.x / 2.0
#aj = imu_data.orientation.y / 2.0
#ak = imu_data.orientation.z / 2.0
#ci = math.cos(ai)
#si = math.sin(ai)
#cj = math.cos(aj)
#sj = math.sin(aj)
#ck = math.cos(ak)
#sk = math.sin(ak)
#quaternion = numpy.empty((4, ), dtype=numpy.float64)
Imu_data.orientation.x = x #si*cj*ck - ci*sj*sk
Imu_data.orientation.y = y #ci*sj*ck + si*cj*sk
Imu_data.orientation.z = z #ci*cj*sk - si*sj*ck
