def update_imu(self):
"""
Reads all characters in the buffer until finding \r\n
Messages should have the following format: "Q: w x y z | A: x y z | G: x y z"
:return:
"""
# Create new message
try:
imuMsg = Imu()
# Set the sensor covariances
imuMsg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
imuMsg.angular_velocity_covariance = [-1., 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.linear_acceleration_covariance = [
-1., 0, 0, 0, 0, 0, 0, 0, 0
]
imu_data = self.read_data()
if len(imu_data) == 0:
self.log("IMU is not answering", 2)
return
q_data = transformations.quaternion_from_euler(
imu_data[2], imu_data[1], imu_data[0])
q1 = Quaternion()
q1.x = float(q_data[0])
q1.y = float(q_data[1])
q1.z = float(q_data[2])
q1.w = float(q_data[3])
q_off = self.imu_offset
new_q = transformations.quaternion_multiply(
[q1.x, q1.y, q1.z, q1.w], [q_off.x, q_off.y, q_off.z, q_off.w])
imuMsg.orientation.x = new_q[0]
imuMsg.orientation.y = new_q[1]
imuMsg.orientation.z = new_q[2]
imuMsg.orientation.w = new_q[3]
# Handle message header
imuMsg.header.frame_id = "base_link_imu"
imuMsg.header.stamp = rospy.Time.now() + rospy.Duration(nsecs=5000)
self.imu_reading = imuMsg
except SerialException as serial_exc:
self.log(
"SerialException while reading from IMU: {}".format(
serial_exc), 3)
self.calibration = True
except ValueError as val_err:
self.log("Value error from IMU data - {}".format(val_err), 5)
self.val_exc = self.val_exc + 1
except Exception as imu_exc:
self.log(imu_exc, 3)
raise imu_exc
def read_position_from_gazebo(self):
try:
gazebo_coordinates = self.get_model_state_srv(
self.ego_vehicle_id, "")
except rospy.ServiceException as e:
rospy.logerr("Error receiving gazebo state: %s", e.message)
gazebo_coordinates = None
if gazebo_coordinates is not None:
roll, pitch, yaw = transformations.euler_from_quaternion([
gazebo_coordinates.pose.orientation.x,
gazebo_coordinates.pose.orientation.y,
gazebo_coordinates.pose.orientation.z,
gazebo_coordinates.pose.orientation.w
])
self.pos_x = gazebo_coordinates.pose.position.x + 2 * cos(yaw)
self.pos_y = gazebo_coordinates.pose.position.y + 2 * sin(yaw)
rotate = transformations.quaternion_from_euler(0, 0, -PI / 2)
rotate_2 = transformations.quaternion_from_euler(
0, 0, 2 * PI - yaw)
angle_rotated = transformations.quaternion_multiply(
rotate_2, rotate)
roll_r, pitch_r, yaw_r = transformations.euler_from_quaternion(
angle_rotated)
self.angle = degrees(yaw_r) + 180
# moveToXY(self, vehID, edgeID, lane, x, y, angle=-1001.0, keepRoute=1)
traci.vehicle.moveToXY(
self.ego_vehicle_id,
traci.vehicle.getRoadID(self.ego_vehicle_id),
traci.vehicle.getLaneIndex(self.ego_vehicle_id), self.pos_x,
self.pos_y, self.angle, 0)
traci.vehicle.setSpeed(self.ego_vehicle_id,
fabs(gazebo_coordinates.twist.linear.x))
def publish(self, imu_msg, nav_msg):
# GET THE POSITION OF THE GPS IN UTM FRAME
lat = nav_msg.latitude
lon = nav_msg.longitude
alt = nav_msg.altitude
utmPoint = utm.fromLatLong(lat, lon, alt).toPoint()
# CONVERT GPS IN UTM FRAME TO ODOM FRAME
x = utmPoint.x - self.datum.x
y = utmPoint.y - self.datum.y
z = utmPoint.z - self.datum.z
# GET THE ROTATION OF THE IMU FROM ODOM FRAME TO UTM FRAME
q_imu = (imu_msg.orientation.x, imu_msg.orientation.y,
imu_msg.orientation.z, imu_msg.orientation.w)
# GET THE ROTATION/TRANSLATION FROM IMU/NAV TO BASE_LINK (BODY->BODY)
try:
imu_trans = self.tfBuffer.lookup_transform(imu_msg.header.frame_id,
"base_link",
imu_msg.header.stamp)
nav_trans = self.tfBuffer.lookup_transform(nav_msg.header.frame_id,
"base_link",
nav_msg.header.stamp)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr(e)
return
# FIRST, ROTATE IMU_LINK TO BASE_LINK
q_rot = (imu_trans.transform.rotation.x,
imu_trans.transform.rotation.y,
imu_trans.transform.rotation.z,
imu_trans.transform.rotation.w)
#
q_base = tf.quaternion_multiply(q_rot, q_imu)
# NEXT, ROTATE GPS_LINK TO BASE_LINK
v_nav = qv_mult(q_base, [
nav_trans.transform.translation.x,
nav_trans.transform.translation.y,
nav_trans.transform.translation.z
])
# FINALLY, APPLY TRANSLATION FROM GPS_LINK TO BASE_LINK
odom_msg = Odometry()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "base_link"
odom_msg.pose.pose.position.x = v_nav[0] + x
odom_msg.pose.pose.position.y = v_nav[1] + y
odom_msg.pose.pose.position.z = v_nav[2] + z
odom_msg.pose.pose.orientation.x = q_base[0]
odom_msg.pose.pose.orientation.y = q_base[1]
odom_msg.pose.pose.orientation.z = q_base[2]
odom_msg.pose.pose.orientation.w = q_base[3]
nav_cov = np.reshape(nav_msg.position_covariance, [3, 3])
imu_cov = np.reshape(imu_msg.orientation_covariance, [3, 3])
cov = block_diag(nav_cov, imu_cov).flatten()
odom_msg.pose.covariance = cov.tolist()
odom_msg.header.stamp = rospy.Time.now()
self.pub.publish(odom_msg)
if self.do_tf:
T = TransformStamped()
T.header = odom_msg.header
T.child_frame_id = odom_msg.child_frame_id
T.transform.translation.x = odom_msg.pose.pose.position.x
T.transform.translation.y = odom_msg.pose.pose.position.y
T.transform.translation.z = odom_msg.pose.pose.position.z
T.transform.rotation = odom_msg.pose.pose.orientation
self.odom_tf.sendTransform(T)
def qv_mult(q1, v1):
#v1 = tf.unit_vector(v1)
q2 = list(v1)
q2.append(0.0)
return tf.quaternion_multiply(tf.quaternion_multiply(q1, q2),
tf.quaternion_conjugate(q1))[:3]