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]