def Publish(self): imu_data = self._hal_proxy.GetImu() imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self._frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.orientation.w = imu_data['orientation']['w'] imu_msg.orientation.x = imu_data['orientation']['x'] imu_msg.orientation.y = imu_data['orientation']['y'] imu_msg.orientation.z = imu_data['orientation']['z'] imu_msg.linear_acceleration.x = imu_data['linear_accel']['x'] imu_msg.linear_acceleration.y = imu_data['linear_accel']['y'] imu_msg.linear_acceleration.z = imu_data['linear_accel']['z'] imu_msg.angular_velocity.x = imu_data['angular_velocity']['x'] imu_msg.angular_velocity.y = imu_data['angular_velocity']['y'] imu_msg.angular_velocity.z = imu_data['angular_velocity']['z'] # Read the x, y, z and heading self._publisher.publish(imu_msg)
def trigger(self): cs = Compass() # create frame id cs.header.frame_id = self.frame_id # create header (provides time stamp) cs.header.stamp = rospy.Time.now() # convert orientation from the CompassSensor.get_heading_lsb byte (heading in the 0-255 range) # to quaternion self.orientation = self.compass.get_sample()*-2.0 * math.pi/180.0 sample = self.compass.get_sample() # create new imu msgs for the compass with time stamp as before imu = Imu() imu.header.frame_id = self.frame_id imu.header.stamp = rospy.Time.now() imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.angular_velocity.z = 1*math.pi/180.0 # the rotation speed # covariance required by robot_pose_ekf imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orienation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() imu.orientation = cs.orientation self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub.publish(imu) # publish the message
def unpackIMU(self, data): imu_msg = Imu() #Unpack Header imu_msg.header = self.unpackIMUHeader(data[0:19]) #Unpack Orientation Message quat = self.bytestr_to_array(data[19:19 + (4*8)]) imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] #Unpack Orientation Covariance imu_msg.orientation_covariance = list(self.bytestr_to_array(data[55:(55 + (9*8))])) #Unpack Angular Velocity ang = self.bytestr_to_array(data[127: 127 + (3*8)]) imu_msg.angular_velocity.x = ang[0] imu_msg.angular_velocity.y = ang[1] imu_msg.angular_velocity.z = ang[2] #Unpack Angular Velocity Covariance imu_msg.angular_velocity_covariance = list(self.bytestr_to_array(data[155:(155 + (9*8))])) #Unpack Linear Acceleration lin = self.bytestr_to_array(data[227: 227 + (3*8)]) imu_msg.linear_acceleration.x = lin[0] imu_msg.linear_acceleration.y = lin[1] imu_msg.linear_acceleration.z = lin[2] #Unpack Linear Acceleration Covariance imu_msg.linear_acceleration_covariance = list(self.bytestr_to_array(data[255:(255 + (9*8))])) return imu_msg
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' #quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz') a = self.kalman_state[3,0] b = self.kalman_state[4,0] c = self.kalman_state[5,0] d = self.kalman_state[6,0] q = math.sqrt(math.pow(a,2)+math.pow(b,2)+math.pow(c,2)+math.pow(d,2)) #angles = tf_math.euler_from_quaternion(self.kalman_state[3:7,0]) imu_msg.orientation.x = a/q#angles[0] imu_msg.orientation.y = b/q imu_msg.orientation.z = c/q imu_msg.orientation.w = d/q imu_msg.orientation_covariance = list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.angular_velocity.x = self.kalman_state[0,0] imu_msg.angular_velocity.y = self.kalman_state[1,0] imu_msg.angular_velocity.z = self.kalman_state[2,0] imu_msg.angular_velocity_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[10,0] imu_msg.linear_acceleration.y = self.kalman_state[11,0] imu_msg.linear_acceleration.z = self.kalman_state[12,0] imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[10:13,10:13].flatten()) self.pub.publish(imu_msg)
def publish_imu_data(): global imu, imu_publisher imu_data = Imu() imu_data.header.frame_id = "imu" imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time) # imu 3dmgx1 reports using the North-East-Down (NED) convention # so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103 # see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros q = imu.orientation imu_data.orientation.w = q[0] imu_data.orientation.x = q[2] imu_data.orientation.y = q[1] imu_data.orientation.z = -q[3] imu_data.orientation_covariance = Config.get_orientation_covariance() av = imu.angular_velocity imu_data.angular_velocity.x = av[1] imu_data.angular_velocity.y = av[0] imu_data.angular_velocity.z = -av[2] imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance() la = imu.linear_acceleration imu_data.linear_acceleration.x = la[1] imu_data.linear_acceleration.y = la[0] imu_data.linear_acceleration.z = - la[2] imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance() imu_publisher.publish(imu_data)
def loop(self): while not rospy.is_shutdown(): line = self.port.readline() chunks = line.split(":") if chunks[0] == "!QUAT": readings = chunks[1].split(",") if len(readings) == 10: imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'imu' imu_msg.orientation.x = float(readings[0]) imu_msg.orientation.y = float(readings[1]) imu_msg.orientation.z = float(readings[2]) imu_msg.orientation.w = float(readings[3]) imu_msg.orientation_covariance = list(zeros((3,3)).flatten()) imu_msg.angular_velocity.x = float(readings[4]) imu_msg.angular_velocity.y = float(readings[5]) imu_msg.angular_velocity.z = float(readings[6]) imu_msg.angular_velocity_covariance = list(0.1*diagflat(ones((3,1))).flatten()) imu_msg.linear_acceleration.x = float(readings[7]) imu_msg.linear_acceleration.y = float(readings[8]) imu_msg.linear_acceleration.z = float(readings[9]) imu_msg.linear_acceleration_covariance = list(0.1*diagflat(ones((3,1))).flatten()) self.pub.publish(imu_msg) quaternion = (imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w) tf_br.sendTransform(translation = (0,0, 0), rotation = quaternion,time = rospy.Time.now(),child = 'imu',parent = 'world') else: rospy.logerr("Did not get a valid IMU packet, got %s", line) else: rospy.loginfo("Did not receive IMU data, instead got %s", line)
def fake_imu(): pub = rospy.Publisher('IMU_ned', Imu, queue_size=10) rospy.init_node('test_convertimu', anonymous=True) rate = rospy.Rate(0.5) # 10hz while not rospy.is_shutdown(): imu_message=Imu() imu_message.header.stamp=rospy.Time.now() imu_message.header.frame_id="IMU" imu_message.orientation.x=1 imu_message.orientation.y=2 imu_message.orientation.z=3 imu_message.orientation.w=4 imu_message.orientation_covariance=[-1,0,0,0,-1,0,0,0,-1] imu_message.linear_acceleration.x=6 imu_message.linear_acceleration.y=7 imu_message.linear_acceleration.z=8 imu_message.linear_acceleration_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6] imu_message.angular_velocity.x=10 imu_message.angular_velocity.y=11 imu_message.angular_velocity.z=12 imu_message.angular_velocity_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6] pub.publish(imu_message) rate.sleep()
def trigger(self): sample = self.gyro.get_sample() gs = Gyro() gs.header.frame_id = self.frame_id gs.header.stamp = rospy.Time.now() gs.calibration_offset.x = 0.0 gs.calibration_offset.y = 0.0 gs.calibration_offset.z = self.offset gs.angular_velocity.x = 0.0 gs.angular_velocity.y = 0.0 gs.angular_velocity.z = (sample-self.offset)*math.pi/180.0 gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] self.pub.publish(gs) imu = Imu() imu.header.frame_id = self.frame_id imu.header.stamp = rospy.Time.now() imu.angular_velocity.x = 0.0 imu.angular_velocity.y = 0.0 imu.angular_velocity.z = 1*math.pi/180.0 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub2.publish(imu)
def _send_one(self, now, angle): msg = Imu() msg.header.stamp = now msg.header.seq = self._seq self._seq += 1 msg.header.frame_id = self._frame_id msg.angular_velocity_covariance = np.zeros(9) msg.orientation_covariance = np.zeros(9) if self._gyro.current_mode == self._gyro.MODE_RATE: msg.angular_velocity.z = angle - self._rate_bias msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2 if self._invert_rotation: msg.angular_velocity.z *= -1 if self._gyro.current_mode == self._gyro.MODE_DTHETA: msg.angular_velocity = angle/self._sample_period - self._rate_bias msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2 if self._invert_rotation: msg.angular_velocity.z *= -1 if self._gyro.current_mode == self._gyro.MODE_INTEGRATED: dt = (now - self._bias_measurement_time).to_sec() corrected_angle = angle - self._rate_bias*dt msg.orientation.w = cos(corrected_angle/2.0) msg.orientation.z = sin(corrected_angle/2.0) if self._invert_rotation: msg.orientation.z *= -1 msg.orientation_covariance[8] = self._sigma_theta*self._sigma_theta self._pub.publish(msg)
def run(self): """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps. The IMU message, if fully filled in, contains information on orientation, acceleration (in m/s^2), and angular rate (in radians/sec). For each of these quantities, the IMU message format also wants the corresponding covariance matrix. Wiimote only gives us acceleration and angular rate. So we ensure that the orientation data entry is marked invalid. We do this by setting the first entry of its associated covariance matrix to -1. The covariance matrices are the 3x3 matrix with the axes' variance in the diagonal. We obtain the variance from the Wiimote instance. """ rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).") self.threadName = "IMU topic Publisher" try: while not rospy.is_shutdown(): (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData() msg = Imu(header=None, orientation=None, # will default to [0.,0.,0.,0], orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.], # -1 indicates that orientation is unknown angular_velocity=None, angular_velocity_covariance=self.angular_velocity_covariance, linear_acceleration=None, linear_acceleration_covariance=self.linear_acceleration_covariance) # If a gyro is plugged into the Wiimote, then note the # angular velocity in the message, else indicate with # the special gyroAbsence_covariance matrix that angular # velocity is unavailable: if self.wiistate.motionPlusPresent: msg.angular_velocity.x = canonicalAngleRate[PHI] msg.angular_velocity.y = canonicalAngleRate[THETA] msg.angular_velocity.z = canonicalAngleRate[PSI] else: msg.angular_velocity_covariance = self.gyroAbsence_covariance msg.linear_acceleration.x = canonicalAccel[X] msg.linear_acceleration.y = canonicalAccel[Y] msg.linear_acceleration.z = canonicalAccel[Z] measureTime = self.wiistate.time timeSecs = int(measureTime) timeNSecs = int(abs(timeSecs - measureTime) * 10**9) msg.header.stamp.secs = timeSecs msg.header.stamp.nsecs = timeNSecs self.pub.publish(msg) rospy.logdebug("IMU state:") rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate)) rospy.sleep(self.sleepDuration) except rospy.ROSInterruptException: rospy.loginfo("Shutdown request. Shutting down Imu sender.") exit(0)
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'quat'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) if(lineParts[0] == 'ypr'): self._ax = float(lineParts[1]) self._ay = float(lineParts[2]) self._az = float(lineParts[3]) if(lineParts[0] == 'areal'): self._lx = float(lineParts[1]) self._ly = float(lineParts[2]) self._lz = float(lineParts[3]) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw imu_msg.angular_velocity.x = self._ax imu_msg.angular_velocity.y = self._ay imu_msg.angular_velocity.z = self._az imu_msg.linear_acceleration.x = self._lx imu_msg.linear_acceleration.y = self._ly imu_msg.linear_acceleration.z = self._lz self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def spin(self): self.prev_time = rospy.Time.now() while not rospy.is_shutdown(): if self.calibrating: self.calibrate() self.calibrating = False self.prev_time = rospy.Time.now() acceldata = self.accelerometer.read6Reg(accel_x_low) compassdata = self.compass.read6Reg(compass_x_high) gyrodata = self.gyro.read6Reg(gyro_x_low) # prepare Imu frame imu = Imu() imu.header.frame_id = self.frame_id self.linear_acceleration = Vector3(); # get line from device #str = self.ser.readline() # timestamp imu.header.stamp = rospy.Time.now() #nums = str.split() # check, if it was correct line #if (len(nums) != 5): # continue self.linear_acceleration.x = self.twosComplement(acceldata[0], acceldata[1]) #/16384.0 self.linear_acceleration.y = self.twosComplement(acceldata[2], acceldata[3]) #/16384.0 self.linear_acceleration.z = self.twosComplement(acceldata[4], acceldata[5]) #/16384.0 imu.orientation.x = self.twosComplement(compassdata[1], compassdata[0]) #/1055.0, imu.orientation.y = self.twosComplement(compassdata[3], compassdata[2]) #/1055.0, imu.orientation.z = self.twosComplement(compassdata[5], compassdata[4]) #/950.0 imu.angular_velocity.x = self.twosComplement(gyrodata[0], gyrodata[1]) imu.angular_velocity.y = self.twosComplement(gyrodata[2], gyrodata[3]) imu.angular_velocity.z = self.twosComplement(gyrodata[4], gyrodata[5]) #gyro = int(nums[2]) #ref = int(nums[3]) #temp = int(nums[4]) #val = (ref-gyro - self.bias) * 1000 / 3 / 1024 * self.scale #imu.angular_velocity.x = 0 #imu.angular_velocity.y = 0 #imu.angular_velocity.z = val * math.pi / 180 imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() self.prev_time = imu.header.stamp (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.pub.publish(imu)
def GetImuFromLine(line): (ts, ax, ay, az, gx, gy, gz) = [t(s) for t,s in zip((int,int, int, int, int, int, int),line.split())] imu = Imu() ts = float(ts)/1000000.0 ImuStamp = rospy.rostime.Time.from_sec(ts) imu.angular_velocity = create_vector3(gyro_raw_to_rads(gx), gyro_raw_to_rads(gy), gyro_raw_to_rads(gz)) imu.angular_velocity_covariance = create_diag_mat(gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0)); imu.linear_acceleration = create_vector3(acc_raw_to_ms(ax), acc_raw_to_ms(ay), acc_raw_to_ms(az)) imu.linear_acceleration_covariance = create_diag_mat(acc_raw_to_ms(1.0), acc_raw_to_ms(1.0), acc_raw_to_ms(1.0)) return (ts, ImuStamp, imu)
def Vn100Pub(): pub = rospy.Publisher('IMUData', Imu, queue_size=1) pub2 = rospy.Publisher('IMUMag', MagneticField, queue_size=1) # Initialize the node and name it. rospy.init_node('IMUpub') vn = imuthread3.Imuthread(port=rospy.get_param("imu_port"), pause=0.0) vn.start() vn.set_data_freq50() vn.set_qmr_mode() #vn.set_data_freq10() #to see if this fixes my gps drop out problem rospy.sleep(3) msg = Imu() msg2 = MagneticField() count = 0 while not rospy.is_shutdown(): if len(vn.lastreadings)>0: if vn.lastreadings[0] =='VNQMR': msg.header.seq = count msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'imu' msg.orientation.x = float(vn.lastreadings[1]) msg.orientation.y = float(vn.lastreadings[2]) msg.orientation.z = float(vn.lastreadings[3]) msg.orientation.w = float(vn.lastreadings[4]) msg.orientation_covariance = [0,0,0,0,0,0,0,0,0] msg.angular_velocity.x = float(vn.lastreadings[8]) msg.angular_velocity.y = float(vn.lastreadings[9]) msg.angular_velocity.z = float(vn.lastreadings[10]) msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0] msg.linear_acceleration.x = float(vn.lastreadings[11]) msg.linear_acceleration.y = float(vn.lastreadings[12]) msg.linear_acceleration.z = float(vn.lastreadings[13]) msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0] msg2.header.seq = count msg2.header.stamp = rospy.Time.now() msg2.header.frame_id = 'imu' msg2.magnetic_field.x = float(vn.lastreadings[5]) msg2.magnetic_field.x = float(vn.lastreadings[6]) msg2.magnetic_field.x = float(vn.lastreadings[7]) msg2.magnetic_field_covariance = [0,0,0,0,0,0,0,0,0] #rospy.loginfo("vn100_pub " + str(msg.header.stamp) + " " + str(msg.orientation.x) + " "+ str(msg.orientation.y) + " "+ str(msg.orientation.z) + " "+ str(msg.orientation.w) + " ") current_pose_euler = tf.transformations.euler_from_quaternion([ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ]) pub.publish(msg) pub2.publish(msg2) count += 1 #rospy.sleep(1.0/100.0) vn.kill = True
def main(): rospy.init_node(NODE_NAME) params = rospy.get_param("/imu".format(NODE_NAME), { 'gyro': { 'zero':[-129.91, 63.81, -102.36], 'covariance':[0,0,0, 0,3.6935414044e-06,0, 0,0,0] }, 'angle':{ 'zero':[0,-0.118467263978,0], 'covariance':[0,0,0, 0,3.57350008404e-05,0, 0,0,0] } }) rospy.set_param("/imu", params) gyro_zeros = np.array(params['gyro']['zero']) gyro_y_variance = params['gyro']['covariance'][4] angle_y_zero = params['angle']['zero'][Y] angle_y_variance = params['angle']['covariance'][4] rate = rospy.Rate(SAMPLE_FREQ_HZ) publisher = rospy.Publisher(IMU_TOPIC, Imu, queue_size=1, tcp_nodelay=True) raw_publisher = rospy.Publisher(IMU_RAW_TOPIC, Imu, queue_size=1, tcp_nodelay=True) rospy.loginfo("Starting {}, publishing at {} hz on {}".format(NODE_NAME, SAMPLE_FREQ_HZ, IMU_TOPIC)) state = Sample() work = Sample() sensor_reading = Sample() raw_msg = Imu() imu_msg = Imu() imu_msg.angular_velocity_covariance = params['gyro']['covariance'] # imu_msg.linear_acceleration_covariance = params['accel']['covariance'] while not rospy.is_shutdown(): read_sensor(sensor_reading) if PUBLISH_RAW: populate_message(raw_msg, sensor_reading) raw_publisher.publish(raw_msg) process_reading(sensor_reading, work, state, gyro_zeros, gyro_y_variance, angle_y_zero, angle_y_variance) populate_message(imu_msg, state) publisher.publish(imu_msg) rate.sleep()
def talker(): global oldTime pubString = rospy.Publisher('sensor/string', String, queue_size=1000) pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000) rospy.init_node('sensornimureader', anonymous=True) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): # sample data currentTime = rospy.Time.now() timeCheck = currentTime.to_sec() - oldTime.to_sec() print timeCheck if (timeCheck) > 2.0: data = 'Z1.341725,103.965008,1.5100,0.0000' oldTime = currentTime; else: data = 'Y0.0000,0.0730,255.4516,1.5100,0.0000,0.0000,0.000,0.000,0.000' # data = ser.readline() pubString.publish(data) if data[0] == 'Y': data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "base_link" imuData.header.stamp = rospy.Time.now() ##data form in yaw, pitch, roll quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6] imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7] imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3] pubIMU.publish(imuData) log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ % (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time()) except: log = "IMU Data Error! Data : %s" % data else: log = "Data Error! Data : %s" % data rospy.loginfo(log) rate.sleep()
def convert_to_baselink(self, imu_msg, imu_model, transform): # convert imu_msg from its frame to baselink frame # Assumption! TODO: I'm going to assume that the axis are aligned between frames to start # come back to this later, and rotate to align axis first # proposed path: # -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link # -> run the same code below ''' [sensor_msgs/Imu]: std_msgs/Header header - DONE uint32 seq time stamp string frame_id geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[9] orientation_covariance geometry_msgs/Vector3 angular_velocity float64 x float64 y float64 z float64[9] angular_velocity_covariance geometry_msgs/Vector3 linear_acceleration - DONE float64 x float64 y float64 z float64[9] linear_acceleration_covariance - DONE ''' new_msg = Imu() # Header new_msg.header = imu_msg.header new_msg.header.frame_id = '/base_link' # Orientation (same based on Assumption! above) new_msg.orientation = imu_msg.orientation # including covariance, because same. will likely drop for rotation new_msg.orientation_covariance = imu_msg.orientation_covariance # Angular Velocity (same based on Assumption! above) new_msg.angular_velocity = imu_msg.angular_velocity # including covariance, because same. will likely drop for rotation new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance # Linear Acceleration (not the same, even with assumption) new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform) return new_msg
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'e'): self._left_encoder_value = long(lineParts[1]) self._right_encoder_value = long(lineParts[2]) self._Left_Encoder.publish(self._left_encoder_value) self._Right_Encoder.publish(self._right_encoder_value) #if(lineParts[0] == 'b'): #self._battery_value = float(lineParts[1]) #self._Battery_Level.publish(self._battery_value) if(lineParts[0] == 'u'): self._ultrasonic_value = float(lineParts[1]) self._Ultrasonic_Value.publish(self._ultrasonic_value) if(lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) self._qx_.publish(self._qx) self._qy_.publish(self._qy) self._qz_.publish(self._qz) self._qw_.publish(self._qw) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def talker(): pub = rospy.Publisher('IMUBoat', Imu, queue_size=1000) rospy.init_node('callibIMU', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): data = ser.readline() # print data # print len(data) if data[0] == 'Y' and len(data) >= 70 and len(data) <= 85: data = data.replace("Y","").replace("\n","").replace("\r","") data_list = data.split(',') if len(data_list) == 9: try: ##data_list structure: accel, magni, gyro float_list = [float(i) for i in data_list] imuData = Imu() imuData.header.frame_id = "imu" imuData.header.stamp = rospy.Time.now() quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx') imuData.orientation.x = quat[0] imuData.orientation.y = quat[1] imuData.orientation.z = quat[2] imuData.orientation.w = quat[3] imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale) imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale) imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale) imuData.linear_acceleration.x = float_list[0]*accel_scale imuData.linear_acceleration.y = -float_list[1]*accel_scale imuData.linear_acceleration.z = -float_list[2]*accel_scale imuData.orientation_covariance = [] imuData.angular_velocity_covariance = [] imuData.linear_acceleration_covariance = [] pub.publish(imuData) # log = "Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \ # % (float_list[0], float_list[1], float_list[2], float_list[3], # float_list[4], float_list[5], float_list[6], float_list[7], # float_list[8], rospy.get_time()) print "%f %f %f %f %f %f %f %f %f" % ( imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z, float_list[3], float_list[4], float_list[5], imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z) except: continue #log = "Data Error! Data : %s" % data else: continue #log = "Data Error! Data : %s" % data #rospy.loginfo(log) else: continue #log = "Non Data Serial Message: %s" % data #rospy.loginfo(log) rate.sleep()
def convertData(self): if self.msgid < self.msgs_len: msg = Imu() msg.header.seq = self.timestamp_ms[self.msgid][0] msg.header.stamp = rospy.Time.from_sec(self.stamp) # TODO: # SCALED_IMU.msg:int16 # HIGHRES_IMU.msg:float32 # RAW_IMU.msg:int16 msg.angular_velocity.x = self.getMsgValue("GyrX") msg.angular_velocity.y = self.getMsgValue("GyrY") msg.angular_velocity.z = self.getMsgValue("GyrZ") msg.linear_acceleration.x = self.getMsgValue("AccX") msg.linear_acceleration.y = self.getMsgValue("AccY") msg.linear_acceleration.z = self.getMsgValue("AccZ") # TODO: This is not enough for ROS IMU msg, add cov matrices and calculate orientation # simple complementary filter for orientation GYROSCOPE_SENSITIVITY = 1 # dt = self.last_stamp - self.stamp #pitch += (msg.angular_velocity.x / GYROSCOPE_SENSITIVITY) * dt # Angle around the X-axis #roll -= (msg.angular_velocity.y / GYROSCOPE_SENSITIVITY) * dt # Angle around the Y-axis # Turning around the X axis results in a vector on the Y-axis #pitchAcc = atan2(msg.linear_acceleration.x, ( sqrt(pow(msg.linear_acceleration.y,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI #pitch = pitch * 0.98 + pitchAcc * 0.02 # Turning around the Y axis results in a vector on the X-axis #rollAcc = atan2(msg.linear_acceleration.y, (sqrt(pow(msg.linear_acceleration.x,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI #roll = roll * 0.98 + rollAcc * 0.02 #yaw = (msg.angular_velocity.z / GYROSCOPE_SENSITIVITY) * dt #orientation.setRPY(roll, pitch, yaw) #tf::quaternionTFToMsg(orientation, orientation_msg) #msg.header.frame_id = frame_id_imu_link; #imu_transform.setRotation(orientation); msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1] msg.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1] #self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec() #self.prev_time = imu.header.stamp #(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion() self.bag.write(self.topic, msg, msg.header.stamp) self.msgid = self.msgid + 1
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) while len(line) > 0: lineParts = line.split("\t") try: if lineParts[0] == "e": self.angle_z = float(lineParts[1]) self._qx = 0.0 self._qy = 0.0 self._qz = math.sin(math.pi * self.angle_z / 360.0) self._qw = math.cos(math.pi * self.angle_z / 360.0) ####################################################################################################################### self._qx_.publish(self._qx) self._qy_.publish(self._qy) self._qz_.publish(self._qz) self._qw_.publish(self._qw) ####################################################################################################################### imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1.0,) * 9 imu_msg.angular_velocity_covariance = (-1.0,) * 9 imu_msg.linear_acceleration_covariance = (-1.0,) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def msg_template(self): msg = Imu() msg.header.frame_id = "robocape" msg.linear_acceleration_covariance = (0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) msg.angular_velocity_covariance = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) msg.orientation_covariance = (10.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 10.0) return msg
def pub_imu(msg_type, msg, bridge): pub = bridge.get_ros_pub("imu", Imu, queue_size=1) imu = Imu() imu.header.stamp = bridge.project_ap_time(msg) imu.header.frame_id = 'base_footprint' # Orientation as a Quaternion, with unknown covariance quat = quaternion_from_euler(msg.roll, msg.pitch, msg.yaw, 'sxyz') imu.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) imu.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) # Angular velocities, with unknown covariance imu.angular_velocity.x = msg.rollspeed imu.angular_velocity.y = msg.pitchspeed imu.angular_velocity.z = msg.yawspeed imu.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) # Not supplied with linear accelerations imu.linear_acceleration_covariance[0] = -1 pub.publish(imu)
def Handle_Received_Line(self, line): self._counter = self._counter + 1 self.SerialPublisher.publish(String(str(self._counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'e'): self._left_encoder_val = long(lineParts[1]) self._right_encoder_val = long(lineParts[2]) self.left_encoder_pub.publish(self._left_encoder_val) self.right_encoder_pub.publish(self._right_encoder_val) if(lineParts[0] == 'u'): self._ultrasonic_val = float(lineParts[1]) self.ultrasonic_pub.publish(self._ultrasonic_val) if(lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) self._qx_pub.publish(self._qx) self._qy_pub.publish(self._qy) self._qz_pub.publish(self._qz) self._qw_pub.publish(self._qw) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in the sensor values") rospy.logwarn(lineParts) pass
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu' imu_msg.orientation = Quaternion(*SF9DOF_UKF.recover_quat(self.kalman_state)) imu_msg.orientation_covariance = \ list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[3,0] imu_msg.angular_velocity.y = self.kalman_state[4,0] imu_msg.angular_velocity.z = self.kalman_state[5,0] imu_msg.angular_velocity_covariance = \ list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[6,0] imu_msg.linear_acceleration.y = self.kalman_state[7,0] imu_msg.linear_acceleration.z = self.kalman_state[8,0] imu_msg.linear_acceleration_covariance = \ list(self.kalman_covariance[6:9,6:9].flatten()) self.pub.publish(imu_msg)
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz') imu_msg.orientation.x = quat[0] imu_msg.orientation.y = quat[1] imu_msg.orientation.z = quat[2] imu_msg.orientation.w = quat[3] imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[3,0] imu_msg.angular_velocity.y = self.kalman_state[4,0] imu_msg.angular_velocity.z = self.kalman_state[5,0] imu_msg.angular_velocity_covariance = list(self.kalman_covariance[3:6,3:6].flatten()) imu_msg.linear_acceleration.x = self.kalman_state[11,0] imu_msg.linear_acceleration.y = self.kalman_state[12,0] imu_msg.linear_acceleration.z = self.kalman_state[13,0] imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[11:14,11:14].flatten())#[.01, 0, 0, 0, .01, 0, 0, 0, .01] self.pub.publish(imu_msg)
def talker(): #Create publisher ('Topic name', msg type) pub = rospy.Publisher('IMU', Imu) #Tells rospy name of the node to allow communication to ROS master rospy.init_node('IMUtalker') while not rospy.is_shutdown(): #Grab relevant information from parse() try: (accel,gyro,magne) = parse() #If data is bad, uses presvious data except: continue #Define IMUmsg to be of the Imu msg type IMUmsg = Imu() #Set header time stamp IMUmsg.header.stamp = rospy.Time.now() #Set orientation parameters IMUmsg.orientation = Quaternion() IMUmsg.orientation.x = magne[0] IMUmsg.orientation.y = magne[1] IMUmsg.orientation.z = magne[2] IMUmsg.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Set angular velocity parameters IMUmsg.angular_velocity = Vector3() IMUmsg.angular_velocity.x = gyro[0] IMUmsg.angular_velocity.y = gyro[1] IMUmsg.angular_velocity.z = gyro[2] IMUmsg.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Set linear acceleration parameters IMUmsg.linear_acceleration = Vector3() IMUmsg.linear_acceleration.x = accel[0] IMUmsg.linear_acceleration.y = accel[1] IMUmsg.linear_acceleration.z = accel[2] IMUmsg.linear_acceleration_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0) #Publish the data pub.publish(IMUmsg)
def run_arduino(self): rospy.init_node('arduino_driver', anonymous=False) print "Arduino Driver is Running!" while not rospy.is_shutdown(): message=self.ser_obj.readline() result=message.strip().split(',') if len(result)==7: imu_message=Imu() imu_message.header.stamp=rospy.Time.now() imu_message.header.frame_id="gyro_link" imu_message.orientation.x=0.0 imu_message.orientation.y=0.0 imu_message.orientation.z=0.0 imu_message.orientation.w=1.0 imu_message.orientation_covariance=[-1, 0, 0, 0, -1, 0, 0, 0, -1] imu_message.linear_acceleration.x=float(result[0]) imu_message.linear_acceleration.y=float(result[1]) imu_message.linear_acceleration.z=float(result[2]) imu_message.linear_acceleration_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] imu_message.angular_velocity.x=float(result[3]) imu_message.angular_velocity.y=float(result[4]) imu_message.angular_velocity.z=float(result[5]) imu_message.angular_velocity_covariance=[1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # accel=result[0:3] # gyro=result[3:6] # batt=result[6] #res=[accel, gyro, batt] # self.imu_pub.publish(imu_message) self.imu_pub.publish(imu_message) self.batt_pub.publish(float(result[6]))
def publish_imu(self): imu_msg = Imu() imu_msg.header.stamp = self.time imu_msg.header.frame_id = 'imu_odom' imu_msg.orientation.x = self.kalman_state[0,0] imu_msg.orientation.y = self.kalman_state[1,0] imu_msg.orientation.z = self.kalman_state[2,0] imu_msg.orientation.w = self.kalman_state[3,0] imu_msg.orientation_covariance = list(self.kalman_covariance[0:3,0:3].flatten()) imu_msg.angular_velocity.x = self.kalman_state[4,0] imu_msg.angular_velocity.y = self.kalman_state[5,0] imu_msg.angular_velocity.z = self.kalman_state[6,0] # Check this covariance imu_msg.angular_velocity_covariance = list(self.kalman_covariance[4:7,4:7].flatten()) imu_msg.linear_acceleration.x = self.measurement[0,0] imu_msg.linear_acceleration.y = self.measurement[1,0] imu_msg.linear_acceleration.z = self.measurement[2,0] acc_cov = SF9DOF_UKF.measurement_noise(self.measurement, 1.0)[0:3,0:3] imu_msg.linear_acceleration_covariance = list(acc_cov.flatten()) self.pub.publish(imu_msg)
def update_sensors(self): # Accelerometer accel = self._driver.get_accelerometer() accel_msg = Imu() accel_msg.header.stamp = rospy.Time.now() accel_msg.header.frame_id = self._name+"/base_link" accel_msg.linear_acceleration.x = (accel[1]-2048.0)/800.0*9.81 # 1 g = about 800, then transforms in m/s^2. accel_msg.linear_acceleration.y = (accel[0]-2048.0)/800.0*9.81 accel_msg.linear_acceleration.z = (accel[2]-2048.0)/800.0*9.81 accel_msg.linear_acceleration_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] #print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2]) #print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81) accel_msg.angular_velocity.x = 0 accel_msg.angular_velocity.y = 0 accel_msg.angular_velocity.z = 0 accel_msg.angular_velocity_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] q = tf.transformations.quaternion_from_euler(0, 0, 0) accel_msg.orientation = Quaternion(*q) accel_msg.orientation_covariance = [0.01,0.0,0.0, 0.0,0.01,0.0, 0.0,0.0,0.01] self.accel_publisher.publish(accel_msg)
def corrimudata_handler(self, corrimudata): # TODO: Work out these covariances properly. Logs provide covariances in local frame, not body imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Populate orientation field with one from inspvax message. imu.orientation = Quaternion(*self.orientation) imu.orientation_covariance = self.orientation_covariance # Angular rates (rad/s) # corrimudata log provides instantaneous rates so multiply by IMU rate in Hz imu.angular_velocity.x = corrimudata.pitch_rate * self.imu_rate imu.angular_velocity.y = corrimudata.roll_rate * self.imu_rate imu.angular_velocity.z = corrimudata.yaw_rate * self.imu_rate imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration (m/s^2) imu.linear_acceleration.x = corrimudata.x_accel * self.imu_rate imu.linear_acceleration.y = corrimudata.y_accel * self.imu_rate imu.linear_acceleration.z = corrimudata.z_accel * self.imu_rate imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu)
pub_imu_pendu_mag = rospy.Publisher('imu_pendu_mag', Vector3Stamped, queue_size = 1) imu_plate_Msg = Imu() imu_plate_mag_Msg = Vector3Stamped() imu_pendu_Msg = Imu() imu_pendu_mag_Msg = Vector3Stamped() imu_plate_Msg.orientation_covariance = [ 0.0025 , 0 , 0, 0, 0.0025, 0, 0, 0, 0.0025 ] imu_plate_Msg.angular_velocity_covariance = [ 0.02, 0 , 0, 0 , 0.02, 0, 0 , 0 , 0.02 ] imu_plate_Msg.linear_acceleration_covariance = [ 0.04 , 0 , 0, 0 , 0.04, 0, 0 , 0 , 0.04 ] imu_pendu_Msg.orientation_covariance = [ 0.0025 , 0 , 0, 0, 0.0025, 0, 0, 0, 0.0025 ]
def start(self): imu_count = 0 speed_count = 0 gnss_count = 0 time = 0 total_time = self.imu_data["t"][0][0][-1][0] while ((not rospy.is_shutdown()) and time < total_time):#checking if the IMU has stopped publishin time = self.imu_data["t"][0][0][imu_count][0] #sending IMU data acc = self.imu_data["acc"][0][0][0:, imu_count] gyro = self.imu_data["gyro"][0][0][0:, imu_count] imuMsg = Imu() imuMsg.header.stamp.secs = time imuMsg.header.frame_id = "base_link" #defining the covariance matrix for IMU measurement imuMsg.angular_velocity_covariance = [ 0.002, 0, 0, 0, 0.002, 0, 0, 0, 0.002 ] imuMsg.linear_acceleration_covariance = [ 0.08, 0, 0, 0, 0.08, 0, 0, 0, 0.08 ] imuMsg.angular_velocity.x = gyro[1] imuMsg.angular_velocity.y = gyro[0] imuMsg.angular_velocity.z = -gyro[2] imuMsg.linear_acceleration.x = acc[1] imuMsg.linear_acceleration.y = acc[0] imuMsg.linear_acceleration.z = -acc[2] self.pub_imu.publish(imuMsg) imu_count = imu_count + 1 # sending Odom data if(self.speedometer_data["t"][0][0][speed_count][0] < time): odomMsg = Odometry() odomMsg.header.stamp.secs = self.speedometer_data["t"][0][0][speed_count][0] odomMsg.header.frame_id = "odom" odomMsg.child_frame_id = "base_link" odomMsg.twist.twist.linear.x = self.speedometer_data["speed"][0][0][0][speed_count] odomMsg.twist.twist.linear.y = 0.0 self.pub_odom.publish(odomMsg) speed_count = speed_count + 1 #sending Map data if (self.gnss_data["t"][0][0][gnss_count][0] < time): gnssMsg = Odometry() ned_pos = self.gnss_data["pos_ned"][0][0][0:, gnss_count] gnssMsg.header.stamp.secs = self.gnss_data["t"][0][0][gnss_count][0] gnssMsg.header.frame_id = "map" gnssMsg.pose.pose.position.x = ned_pos[1] gnssMsg.pose.pose.position.y = ned_pos[0] gnssMsg.pose.pose.position.z = 0.0 self.pub_gnss.publish(gnssMsg) gnss_count = gnss_count + 1 self.looprate.sleep()
def publishState(self, state, ros_pub_dec, numDoF): # Publish our robot state to ROS topics /robotname/state/* periodically self.publish_time += 1 if bROS and self.publish_time > ros_pub_dec: publish_time = 0 # Construct /robotname/state/imu ROS message msg = Imu() msg.linear_acceleration.x = state['imu/linear_acceleration'][0] msg.linear_acceleration.y = state['imu/linear_acceleration'][1] msg.linear_acceleration.z = state['imu/linear_acceleration'][2] msg.angular_velocity.x = state['imu/angular_velocity'][0] msg.angular_velocity.y = state['imu/angular_velocity'][1] msg.angular_velocity.z = state['imu/angular_velocity'][2] msg.orientation_covariance = state['imu/orientation_covariance'] msg.linear_acceleration_covariance = np.empty(9) msg.linear_acceleration_covariance.fill( state['imu/linear_acceleration_covariance']) msg.angular_velocity_covariance = np.empty(9) msg.angular_velocity_covariance.fill( state['imu/angular_velocity_covariance']) roll, pitch, yaw = state[ 'imu/euler'] # Convert from euler to quaternion using ROS tf quaternion = tf.transformations.quaternion_from_euler( roll, pitch, yaw) msg.orientation.x = quaternion[0] msg.orientation.y = quaternion[1] msg.orientation.z = quaternion[2] msg.orientation.w = quaternion[3] self.pubs[0].publish(msg) # Construct /robotname/state/pose msg = Pose() msg.orientation.x = quaternion[0] msg.orientation.y = quaternion[1] msg.orientation.z = quaternion[2] msg.orientation.w = quaternion[3] # TODO: Get height from robot state, have robot calculate it msg.position.z = 0.0 self.pubs[7].publish(msg) # Construct /robotname/state/batteryState msg = BatteryState() msg.current = state['battery/current'] msg.voltage = state['battery/voltage'] #num_cells = 8 num_cells = 4 if 'battery/cell_voltage' in state: if state['battery/cell_voltage'] > 0: num_cells = len(state['battery/cell_voltage']) # FIXME: do i really need this? def percentage(total_voltage, num_cells): # Linearly interpolate charge from voltage # https://gitlab.com/ghostrobotics/SDK/uploads/6878144fa0e408c91e481c2278215579/image.png charges = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0] voltages = [3.2, 3.5, 3.6, 3.65, 3.8, 4.2] return np.interp(total_voltage / num_cells, voltages, charges) if msg.percentage <= 0: msg.percentage = percentage(msg.voltage, num_cells) self.pubs[1].publish(msg) # Construct /robotname/state/behaviorId msg = UInt32() msg.data = state['behaviorId'] self.pubs[2].publish(msg) # Construct /robotname/state/behaviorMode msg = UInt32() msg.data = state['behaviorMode'] self.pubs[3].publish(msg) # Construct /robotname/state/joint msg = JointState() msg.name = [] msg.position = [] msg.velocity = [] msg.effort = [] for j in range(len(state['joint/position'])): msg.name.append(str(j)) msg.position.append(state['joint/position'][j]) msg.velocity.append(state['joint/velocity'][j]) msg.effort.append(state['joint/effort'][j]) self.pubs[4].publish(msg) msg.position = convert_to_leg_model(msg.position) # Translate for URDF in NGR # vision60 = False # if(vision60): # for i in range(8, 2): # msg.position[i] += msg.position[i-1]; # msg.velocity[i] += msg.velocity[i-1]; # else: # # other URDF # # for URDF of Minitaur FIXME use the class I put in ethernet.py for RobotType # msg.position.extend([0, 0, 0, 0, 0, 0, 0, 0]) # msg.position[11], msg.position[10], r = minitaurFKForURDF(msg.position[0], msg.position[1]) # msg.position[14], msg.position[15], r = minitaurFKForURDF(msg.position[2], msg.position[3]) # msg.position[9], msg.position[8], r = minitaurFKForURDF(msg.position[4], msg.position[5]) # msg.position[13], msg.position[12], r = minitaurFKForURDF(msg.position[6], msg.position[7]) # # other URDF problems (order important) # for j in range(4): # msg.position[j] = -msg.position[j] self.pubs[5].publish(msg) # Construct /robotname/state/joystick msg = Joy() msg.axes = state['joy/axes'] msg.buttons = state['joy/buttons'] self.pubs[6].publish(msg)
imu_pub = rospy.Publisher('imu', Imu, queue_size=1) reset() # Try sleeping for some time before running calibrate to avoid sudden wheel movement # interfering with calibration. sleep(0.2) # Run a calibration before starting to send data to the robot. cal_values = calibrate() imu_msg = Imu() imu_msg.angular_velocity_covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, get_variance('z', cal_values) ] imu_msg.linear_acceleration_covariance = [ get_variance('x', cal_values), get_covariance('x', cal_values), 0, get_covariance('y', cal_values), get_variance('y', cal_values), 0, 0, 0, 0 ] idx = 0 rawdata = [] sensor_data = [0, 0, 0] rate = rospy.Rate(50)
def talker(): orientation_covariance = [0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] angular_velocity_covariance = [0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] linear_acceleration_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] publisher_left = rospy.Publisher('wheel_speed_left', Int32, queue_size=1) publisher_right = rospy.Publisher('wheel_speed_right', Int32, queue_size=1) publisher_twist = rospy.Publisher('wheel_speed_twist', Twist, queue_size=1) publisher_sonar = rospy.Publisher('data_sonar', Vector3, queue_size=1) publisher_bumper = rospy.Publisher('data_bumper', Vector3, queue_size=1) publisher_imu = rospy.Publisher('data_imu', Imu, queue_size=1) # publisher_imu_angular_velocities = rospy.Publisher( # 'data_imu_angular_velocities', Vector3, queue_size=1) # publisher_imu_accelerations = rospy.Publisher( # 'data_imu_accelerations', Vector3, queue_size=1) # publisher_imu_quaternion = rospy.Publisher( # 'data_imu_quaternion', Quaternion, queue_size=1) rospy.init_node('publish_robot_data') ser = open_port() print(ser) with ser: while not rospy.is_shutdown(): line = ser.readline().rstrip() if len(line) > 4: if line[0] == 'W' and line[1] == '['\ and line[len(line) - 1] == 'W'\ and line[len(line) - 2] == ']': parsed_values = parse_data_wheels(line[2:len(line) - 2]) if parsed_values[0]: publisher_left.publish(parsed_values[2]) publisher_right.publish(parsed_values[3]) publisher_twist.publish(parsed_values[1]) elif line[0] == 'S' and line[1] == '['\ and line[len(line) - 1] == 'S'\ and line[len(line) - 2] == ']': parsed_values = parse_data_sonar(line[2:len(line) - 2]) if parsed_values[0]: publisher_sonar.publish(parsed_values[1]) elif line[0] == 'B' and line[1] == '['\ and line[len(line) - 1] == 'B'\ and line[len(line) - 2] == ']': parsed_values = parse_data_bumper(line[2:len(line) - 2]) if parsed_values[0]: publisher_bumper.publish(parsed_values[1]) elif line[0] == 'I' and line[1] == '['\ and line[len(line) - 1] == 'I'\ and line[len(line) - 2] == ']': parsed_values = parse_data_imu(line[2:len(line) - 2]) if parsed_values[0]: imu_angular_velocities = parsed_values[1] imu_accelerations = parsed_values[2] imu_quaternion = parsed_values[3] imu_message = Imu() now = rospy.Time.now() imu_message.header.stamp = now imu_message.header.frame_id = 'imu' imu_message.angular_velocity = imu_angular_velocities imu_message.linear_acceleration = imu_accelerations imu_message.orientation = imu_quaternion imu_message.linear_acceleration_covariance = linear_acceleration_covariance imu_message.angular_velocity_covariance = angular_velocity_covariance imu_message.orientation_covariance = orientation_covariance publisher_imu.publish(imu_message) # publisher_imu_angular_velocities.publish( # parsed_values[1]) # publisher_imu_accelerations.publish(parsed_values[2]) # publisher_imu_quaternion.publish(parsed_values[3]) else: print("Cannot parse data from the string") else: print("Received string is empty")
def main(argv): if len(sys.argv) < 2: print 'Please specify data directory file' return 1 if len(sys.argv) < 3: print 'Please specify output rosbag file' return 1 print("loading files...") bag = rosbag.Bag(sys.argv[2], 'w') gps = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/gps.csv", delimiter=",") imu_100hz = np.loadtxt(sys.argv[1] + "sensor_data/2012-01-08/imu_100hz.csv", delimiter=",") ral_seq = 0 bap_seq = 0 img_seq = 0 imu_seq = 0 cal = -1 gps_seq = 0 # IMAGE_COUNT = 81169 STREET_VIEW = 113 print("package gps and image...") print("Packaging GPS and image") for gps_data in gps: utime = int(gps_data[0]) mode = int(gps_data[1]) timestamp = rospy.Time.from_sec(utime / 1e6) lat = float(gps_data[3]) lng = float(gps_data[4]) alt = float(gps_data[5]) status = NavSatStatus() if mode == 0 or mode == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = float(gps_data[2]) fix = NavSatFix() fix.header.seq = gps_seq fix.status = status fix.latitude = np.rad2deg(lat) fix.longitude = np.rad2deg(lng) fix.altitude = np.rad2deg(alt) track = Float64() track.data = float(gps_data[6]) speed = Float64() speed.data = float(gps_data[7]) bag.write('/gps', fix, t=timestamp) bag.write('/gps_track', track, t=timestamp) bag.write('/gps_speed', speed, t=timestamp) # write aerial image if gps_seq <= MAVIMAGE: img_path = sys.argv[1] + 'images/2012-01-08/lb3/Cam5/' img_list = os.listdir(img_path) img_list.sort() img_cv = cv2.imread(os.path.join(img_path, img_list[gps_seq]), -1) img_cv = cv2.resize(img_cv, MAVDIM, interpolation=cv2.INTER_AREA) # 顺时针旋转90度 trans_img = cv2.transpose(img_cv) img_cv = cv2.flip(trans_img, 1) br = CvBridge() Img = Image() Img = br.cv2_to_imgmsg(img_cv, "bgr8") Img.header.seq = int(gps_seq) print(gps_seq) Img.header.stamp = timestamp Img.header.frame_id = 'camera' bag.write('/image/cam5', Img, t=timestamp) gps_seq = gps_seq + 1 print('packaging imu...') for imu_data in imu_100hz: imu_seq = imu_seq + 1 utime = int(imu_data[0]) timestamp = rospy.Time.from_sec(utime / 1e6) imu = Imu() imu.header.seq = imu_seq imu.header.stamp = timestamp imu.header.frame_id = '/Imu' imu.linear_acceleration.x = float(imu_data[5]) imu.linear_acceleration.y = float(imu_data[6]) imu.linear_acceleration.z = float(imu_data[7]) imu.linear_acceleration_covariance = np.zeros(9) imu.angular_velocity.x = float(imu_data[8]) imu.angular_velocity.y = float(imu_data[9]) imu.angular_velocity.z = float(imu_data[10]) imu.angular_velocity_covariance = np.zeros(9) imu.orientation.w = float(imu_data[1]) imu.orientation.x = float(imu_data[2]) imu.orientation.y = float(imu_data[3]) imu.orientation.z = float(imu_data[4]) bag.write('/Imu', imu, t=timestamp) bag.close() return 0
def imunode(): global freq global accel, gyro global R, q, rpyi R = np.eye(3) q = np.array([0, 0, 0, 1]) rpy = np.array([[0], [0], [0]]) imu_msg = Imu() imu_msg.header.frame_id = "/imu" #imu_msg.header.frame_id = "world" odom_msg = Odometry() odom_msg.header.frame_id = "world" odom_msg.child_frame_id = "EspeleoRobo" gps_msg = NavSatFix() gps_msg.header.frame_id = "/imu" pub_imu = rospy.Publisher("/imu/raw", Imu, queue_size=1) pub_gps = rospy.Publisher("/fix", NavSatFix, queue_size=1) rospy.init_node("xsens_emulator") rospy.Subscriber("/sensors/acc", Point, callback_acc) rospy.Subscriber("/sensors/gyro", Point, callback_gyro) rospy.Subscriber("/tf", TFMessage, callback_pose_tf) # ground thruth - from tf transform rate = rospy.Rate(freq) sleep(0.5) print "\33[92mXsens emulator initialized!\33[0m" i = 0 while not rospy.is_shutdown(): i = i + 1 time = i / float(freq) #Publish imu data imu_msg.header.stamp = rospy.Time.now() imu_msg.angular_velocity.x = gyro[0] imu_msg.angular_velocity.y = gyro[1] imu_msg.angular_velocity.z = gyro[2] imu_msg.angular_velocity_covariance = [ 0.2, 0.1, 0.1, 0.1, 0.2, 0.1, 0.1, 0.1, 0.2 ] imu_msg.linear_acceleration.x = accel[0] imu_msg.linear_acceleration.y = accel[1] imu_msg.linear_acceleration.z = accel[2] imu_msg.linear_acceleration_covariance = [ 0.2, 0.1, 0.1, 0.1, 0.2, 0.1, 0.1, 0.1, 0.2 ] pub_imu.publish(imu_msg) # ---------- ---------- ---------- ---------- ---------- #Publish gps data gps_msg.header.stamp = rospy.Time.now() gps_msg.latitude = gps[1] gps_msg.longitude = gps[0] gps_msg.altitude = gps[2] pub_gps.publish(gps_msg) # ---------- ---------- ---------- ---------- ---------- """ # Create odom message odom_msg.header.stamp = rospy.Time.now(); #odom_msg.pose.pose.position.x = p[0] #odom_msg.pose.pose.position.y = p[1] #odom_msg.pose.pose.position.z = p[2] odom_msg.pose.pose.orientation.x = q[0] odom_msg.pose.pose.orientation.y = q[1] odom_msg.pose.pose.orientation.z = q[2] odom_msg.pose.pose.orientation.w = q[3] #Publish odom message pub_odom.publish(odom_msg) """ rate.sleep()
#Sensors data ROS buffers orientation_msg = Vector3(0, 0, 0) angular_vel_msg = Vector3(0, 0, 0) lin_accel_msg = Vector3(0, 0, 0) #Sensors data dictionary sensor_data_dict = { 'A': lin_accel_msg, 'G': angular_vel_msg, 'C': orientation_msg } #ROS IMU message imuMsg = Imu() imuMsg.orientation_covariance = orientation_covariance imuMsg.angular_velocity_covariance = angular_velocity_covariance imuMsg.linear_acceleration_covariance = linear_acceleration_covariance imuMsg.orientation = orientation_msg imuMsg.angular_velocity = angular_vel_msg imuMsg.linear_acceleration = lin_accel_msg """ Node must be able of receiving STM32 sensor messages through serial port, and transform them to a ROS IMU message. The data received through the serial port has the following text format: <Type> ':' <Data> <Data> ... <Data> '\r' '\n' "Type" must be one of these letters: A (accelerometer), C (compass), G (gyroscope). """
def spin_once(self): def baroPressureToHeight(value): c1 = 44330.0 c2 = 9.869232667160128300024673081668e-6 c3 = 0.1901975534856 intermediate = 1 - math.pow(c2 * value, c3) height = c1 * intermediate return height # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) #temp = data.get('Temp') # float orient_data = data.get('Orientation Data') velocity_data = data.get('Velocity') position_data = data.get('Latlon') altitude_data = data.get('Altitude') acc_data = data.get('Acceleration') gyr_data = data.get('Angular Velocity') mag_data = data.get('Magnetic') pressure_data = data.get('Pressure') time_data = data.get('Timestamp') gnss_data = data.get('Gnss PVT') status = data.get('Status') # int # create messages and default values "Imu message supported with Modes 1 & 2" imu_msg = Imu() pub_imu = False "SensorSample message supported with Mode 2" ss_msg = sensorSample() pub_ss = False "Mag message supported with Modes 1 & 2" mag_msg = Vector3Stamped() pub_mag = False "Baro in meters" baro_msg = baroSample() pub_baro = False "GNSS message supported only with MTi-G-7xx devices" "Valid only for modes 1 and 2" gnssPvt_msg = gnssSample() pub_gnssPvt = False #gnssSatinfo_msg = GPSStatus() #pub_gnssSatinfo = False # All filter related outputs "Supported in mode 3" ori_msg = orientationEstimate() pub_ori = False "Supported in mode 3 for MTi-G-7xx devices" vel_msg = velocityEstimate() pub_vel = False "Supported in mode 3 for MTi-G-7xx devices" pos_msg = positionEstimate() pub_pos = False secs = 0 nsecs = 0 if time_data: # first getting the sampleTimeFine time = time_data['SampleTimeFine'] secs = 100e-6 * time nsecs = 1e5 * time - 1e9 * math.floor(secs) if acc_data: if 'Delta v.x' in acc_data: # found delta-v's pub_ss = True ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] elif 'accX' in acc_data: # found acceleration #COLLIN modified pub_imu = True imu_msg.linear_acceleration.x = -acc_data['accX'] imu_msg.linear_acceleration.y = -acc_data['accY'] imu_msg.linear_acceleration.z = -acc_data['accZ'] imu_msg.linear_acceleration_covariance = [ 5e-2, 0, 0, 0, 5e-2, 0, 0, 0, 5e-2 ] else: raise MTException( "Unsupported message in XDI_AccelerationGroup.") if gyr_data: if 'Delta q0' in gyr_data: # found delta-q's pub_ss = True ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] elif 'gyrX' in gyr_data: # found rate of turn pub_imu = True imu_msg.angular_velocity.x = gyr_data['gyrX'] imu_msg.angular_velocity.y = gyr_data['gyrY'] imu_msg.angular_velocity.z = gyr_data['gyrZ'] #COLLIN modified imu_msg.angular_velocity_covariance = [ 5e-2, 0, 0, 0, 5e-2, 0, 0, 0, 5e-2 ] else: raise MTException( "Unsupported message in XDI_AngularVelocityGroup.") if mag_data: # magfield ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX'] ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY'] ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ'] pub_mag = True if pressure_data: pub_baro = True height = baroPressureToHeight(pressure_data['Pressure']) baro_msg.height = ss_msg.internal.baro.height = height if gnss_data: pub_gnssPvt = True gnssPvt_msg.itow = gnss_data['iTOW'] gnssPvt_msg.fix = gnss_data['fix'] gnssPvt_msg.latitude = gnss_data['lat'] gnssPvt_msg.longitude = gnss_data['lon'] gnssPvt_msg.hEll = gnss_data['hEll'] gnssPvt_msg.hMsl = gnss_data['hMsl'] gnssPvt_msg.vel.x = gnss_data['velE'] gnssPvt_msg.vel.y = gnss_data['velN'] gnssPvt_msg.vel.z = -gnss_data['velD'] gnssPvt_msg.hAcc = gnss_data['horzAcc'] gnssPvt_msg.vAcc = gnss_data['vertAcc'] gnssPvt_msg.sAcc = gnss_data['speedAcc'] gnssPvt_msg.pDop = gnss_data['PDOP'] gnssPvt_msg.hDop = gnss_data['HDOP'] gnssPvt_msg.vDop = gnss_data['VDOP'] gnssPvt_msg.numSat = gnss_data['nSat'] gnssPvt_msg.heading = gnss_data['heading'] gnssPvt_msg.headingAcc = gnss_data['headingAcc'] if orient_data: if 'Q0' in orient_data: pub_imu = True imu_msg.orientation.w = orient_data['Q0'] imu_msg.orientation.x = orient_data['Q1'] imu_msg.orientation.y = orient_data['Q2'] imu_msg.orientation.z = orient_data['Q3'] elif 'Roll' in orient_data: pub_ori = True ori_msg.roll = orient_data['Roll'] ori_msg.pitch = orient_data['Pitch'] ori_msg.yaw = orient_data['Yaw'] else: raise MTException( 'Unsupported message in XDI_OrientationGroup') if velocity_data: pub_vel = True vel_msg.velE = velocity_data['velX'] vel_msg.velN = velocity_data['velY'] vel_msg.velU = velocity_data['velZ'] if position_data: pub_pos = True pos_msg.latitude = position_data['lat'] pos_msg.longitude = position_data['lon'] if altitude_data: pub_pos = True tempData = altitude_data['ellipsoid'] pos_msg.hEll = tempData[0] #if status is not None: # if status & 0b0001: # self.stest_stat.level = DiagnosticStatus.OK # self.stest_stat.message = "Ok" # else: # self.stest_stat.level = DiagnosticStatus.ERROR # self.stest_stat.message = "Failed" # if status & 0b0010: # self.xkf_stat.level = DiagnosticStatus.OK # self.xkf_stat.message = "Valid" # else: # self.xkf_stat.level = DiagnosticStatus.WARN # self.xkf_stat.message = "Invalid" # if status & 0b0100: # self.gps_stat.level = DiagnosticStatus.OK # self.gps_stat.message = "Ok" # else: # self.gps_stat.level = DiagnosticStatus.WARN # self.gps_stat.message = "No fix" # self.diag_msg.header = h # self.diag_pub.publish(self.diag_msg) # publish available information if pub_imu: imu_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time imu_msg.header.stamp.secs = secs imu_msg.header.stamp.nsecs = nsecs self.imu_pub.publish(imu_msg) #if pub_gps: # xgps_msg.header = gps_msg.header = h # self.gps_pub.publish(gps_msg) # self.xgps_pub.publish(xgps_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) #if pub_temp: # self.temp_pub.publish(temp_msg) if pub_ss: ss_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ss_msg.header.stamp.secs = secs ss_msg.header.stamp.nsecs = nsecs self.ss_pub.publish(ss_msg) if pub_baro: baro_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.baro_pub.publish(baro_msg) if pub_gnssPvt: gnssPvt_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.gnssPvt_pub.publish(gnssPvt_msg) if pub_ori: ori_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ori_msg.header.stamp.secs = secs ori_msg.header.stamp.nsecs = nsecs self.ori_pub.publish(ori_msg) if pub_vel: vel_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time vel_msg.header.stamp.secs = secs vel_msg.header.stamp.nsecs = nsecs self.vel_pub.publish(vel_msg) if pub_pos: pos_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time pos_msg.header.stamp.secs = secs pos_msg.header.stamp.nsecs = nsecs self.pos_pub.publish(pos_msg)
S_Compass_Declination = rospy.get_param('~declination',0.2333333333334*(math.pi/180.0)) S_Compass_EastAsZero = rospy.get_param('~UseEastAsZero',True) Checksum_error_limits = rospy.get_param('~Checksum_error_limits', 15) checksum_error_counter=0 #------------------------------------------------------------------------------------------------------------- imu_data = Imu() imu_data = Imu(header=rospy.Header(frame_id="SpartonCompassIMU")) # Initialize Imu message imu_data.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] imu_data.angular_velocity_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] imu_data.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] # Instruction lines Dropline='\r\n\r\n print trigger 0 set drop \r\n' Checkline='\r\n printmask gyrop_trigger accelp_trigger or quat_trigger or time_trigger or set drop \r\n' ModulusLine='printmodulus %i set drop \r\n' % S_Printmodulus Streamline='printtrigger printmask set drop \r\n' GSRline='$PSRFS,gyroSampleRate,get' rospy.on_shutdown(Shutdown) try:
#!/usr/bin/env python import roslib import rospy from crius_imu.msg import ImuMin from sensor_msgs.msg import Imu rospy.init_node('imuraw2imu_node') imu_msg = Imu() imu_msg.orientation_covariance = (1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0) imu_msg.angular_velocity_covariance = (1.25e-07, 0, 0, 0, 1.25e-07, 0, 0, 0, 1.25e-07) imu_msg.linear_acceleration_covariance = (8.99e-08, 0, 0, 0, 8.99e-08, 0, 0, 0, 8.99e-08) pub = rospy.Publisher('/imu/data', Imu, queue_size=1) def callback(data): imu_msg.header.stamp = rospy.get_rostime() imu_msg.header.frame_id = "imu" imu_msg.orientation.x = data.quat_x imu_msg.orientation.y = data.quat_y imu_msg.orientation.z = data.quat_z imu_msg.orientation.w = data.quat_w imu_msg.angular_velocity.x = data.gyro_x imu_msg.angular_velocity.y = data.gyro_y imu_msg.angular_velocity.z = data.gyro_z imu_msg.linear_acceleration.x = data.acc_x imu_msg.linear_acceleration.y = data.acc_y imu_msg.linear_acceleration.z = data.acc_z pub.publish(imu_msg)
def __init__(self): self.DEVICE_ID = rospy.get_param( '~device_id', '/dev/serial/by-id/usb-SparkFun_SFE_9DOF-D21-if00') self.imu_pub = rospy.Publisher('imu/camera_tilt', Imu, queue_size=1) self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size=1) imu = Imu() mag = MagneticField() ser = serial.Serial(self.DEVICE_ID) gyro = [0.0, 0.0, 0.0, 0.0] magn = [0.0, 0.0, 0.0, 0.0] quat = [0.0, 1.0, 0.0, 0.0, 0.0] rate = rospy.Rate(130) while not rospy.is_shutdown(): line = ser.readline() accel = line.split(",", 4) if accel[0] == 'A': line = ser.readline() gyro = line.split(",", 4) line = ser.readline() magn = line.split(",", 4) line = ser.readline() quat = line.split(",", 5) imu.header.stamp = rospy.Time.now() imu.header.frame_id = '/base_link' # i.e. '/odom' #pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint' imu.angular_velocity.x = float(gyro[1]) imu.angular_velocity.y = float(gyro[2]) imu.angular_velocity.z = float(gyro[3]) imu.angular_velocity_covariance = [ .001, 0.0, 0.0, 0.0, .001, 0.0, 0.0, 0.0, .001 ] imu.linear_acceleration.x = float(accel[1]) imu.linear_acceleration.y = float(accel[2]) imu.linear_acceleration.z = float(accel[3]) imu.linear_acceleration_covariance = [ .01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01 ] '''imu.orientation.w = float(quat[1]) imu.orientation.x = float(quat[2]) imu.orientation.y = float(quat[3]) imu.orientation.z = float(quat[4]) imu.orientation_covariance = [.01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01] ''' mag.header.stamp = rospy.Time.now() mag.header.frame_id = '/base_link' # i.e. '/odom' #pres.child_frame_id = self.child_frame_id # i.e. '/base_footprint' mag.magnetic_field.x = float(magn[1]) mag.magnetic_field.y = float(magn[2]) mag.magnetic_field.z = float(magn[3]) mag.magnetic_field_covariance = [ .001, 0.0, 0.0, 0.0, .001, 0.0, 0.0, 0.0, .001 ] self.imu_pub.publish(imu) self.mag_pub.publish(mag) rate.sleep()
def loop(self): #Topic 1: PoseWitchCovariance pwc = PoseWithCovarianceStamped() pwc.header.stamp = rospy.get_rostime() pwc.header.frame_id = self.world_frame_id pwc.pose.pose.position = Coordinates() pwc.pose.pose.orientation = pypozyx.Quaternion() cov = pypozyx.PositionError() status = self.pozyx.doPositioning(pwc.pose.pose.position, self.dimension, self.height, self.algorithm, self.tag_device_id) pozyx.getQuaternion(pwc.pose.pose.orientation, self.tag_device_id) pozyx.getPositionError(cov, self.tag_device_id) cov_row1 = [cov.x, cov.xy, cov.xz, 0, 0, 0] cov_row2 = [cov.xy, cov.y, cov.yz, 0, 0, 0] cov_row3 = [cov.xz, cov.yz, cov.z, 0, 0, 0] cov_row4 = [0, 0, 0, 0, 0, 0] cov_row5 = [0, 0, 0, 0, 0, 0] cov_row6 = [0, 0, 0, 0, 0, 0] pwc.pose.covariance = cov_row1 + cov_row2 + cov_row3 + cov_row4 + cov_row5 + cov_row6 #Convert from mm to m pwc.pose.pose.position.x = pwc.pose.pose.position.x * 0.001 pwc.pose.pose.position.y = pwc.pose.pose.position.y * 0.001 pwc.pose.pose.position.z = pwc.pose.pose.position.z * 0.001 if status == POZYX_SUCCESS: pub_pose_with_cov.publish(pwc) #Topic 2: IMU imu = Imu() imu.header.stamp = rospy.get_rostime() imu.header.frame_id = self.tag_frame_id imu.orientation = pypozyx.Quaternion() imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imu.angular_velocity = pypozyx.AngularVelocity() imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] imu.linear_acceleration = pypozyx.LinearAcceleration() imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] pozyx.getQuaternion(imu.orientation, self.tag_device_id) pozyx.getAngularVelocity_dps(imu.angular_velocity, self.tag_device_id) pozyx.getLinearAcceleration_mg(imu.linear_acceleration, self.tag_device_id) #Convert from mg to m/s2 imu.linear_acceleration.x = imu.linear_acceleration.x * 0.0098 imu.linear_acceleration.y = imu.linear_acceleration.y * 0.0098 imu.linear_acceleration.z = imu.linear_acceleration.z * 0.0098 #Convert from Degree/second to rad/s imu.angular_velocity.x = imu.angular_velocity.x * 0.01745 imu.angular_velocity.y = imu.angular_velocity.y * 0.01745 imu.angular_velocity.z = imu.angular_velocity.z * 0.01745 pub_imu.publish(imu) #Topic 3: Anchors Info for i in range(len(anchors)): dr = AnchorInfo() dr.header.stamp = rospy.get_rostime() dr.header.frame_id = self.world_frame_id dr.id = hex(anchors[i].network_id) dr.position.x = (float)(anchors[i].pos.x) * 0.001 dr.position.y = (float)(anchors[i].pos.y) * 0.001 dr.position.z = (float)(anchors[i].pos.z) * 0.001 iter_ranging = 0 while iter_ranging < self.do_ranging_attempts: device_range = DeviceRange() status = self.pozyx.doRanging(self.anchors[i].network_id, device_range, self.tag_device_id) dr.distance = (float)(device_range.distance) * 0.001 dr.RSS = device_range.RSS if status == POZYX_SUCCESS: dr.status = True self.range_error_counts[i] = 0 iter_ranging = self.do_ranging_attempts else: dr.status = False self.range_error_counts[i] += 1 if self.range_error_counts[i] > 9: self.range_error_counts[i] = 0 rospy.logerr("Anchor %d (%s) lost", i, dr.id) iter_ranging += 1 # device_range = DeviceRange() # status = self.pozyx.doRanging(self.anchors[i].network_id, device_range) # dr.distance = (float)(device_range.distance) * 0.001 # dr.RSS = device_range.RSS # if status == POZYX_SUCCESS: # dr.status = True # self.range_error_counts[i] = 0 # else: # status = self.pozyx.doRanging(self.anchors[i].network_id, device_range) # dr.distance = (float)(device_range.distance) * 0.001 # dr.RSS = device_range.RSS # if status == POZYX_SUCCESS: # dr.status = True # self.range_error_counts[i] = 0 # else: # dr.status = False # self.range_error_counts[i] += 1 # if self.range_error_counts[i] > 9: # self.range_error_counts[i] = 0 # rospy.logerr("Anchor %d (%s) lost", i, dr.id) dr.child_frame_id = "anchor_" + str(i) pub_anchor_info[i].publish(dr) #Topic 4: PoseStamped ps = PoseStamped() ps.header.stamp = rospy.get_rostime() ps.header.frame_id = self.world_frame_id ps.pose.position = pwc.pose.pose.position ps.pose.orientation = pwc.pose.pose.orientation pub_pose.publish(ps) #Topic 5: Pressure pr = FluidPressure() pr.header.stamp = rospy.get_rostime() pr.header.frame_id = self.tag_frame_id pressure = pypozyx.Pressure() pozyx.getPressure_Pa(pressure, self.tag_device_id) pr.fluid_pressure = pressure.value pr.variance = 0 pub_pressure.publish(pr)
imuMsg.orientation.w = real if mag_accuracy == 0: imuMsg.orientation_covariance = [ 0.0001, 0.00, 0.00, 0.00, 0.0001, 0.00, 0.00, 0.00, 0.0001 ] else: imuMsg.orientation_covariance = [ 0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01 ] # Gyroscope data (in degrees per second): imuMsg.angular_velocity.x = gyroX imuMsg.angular_velocity.y = gyroY imuMsg.angular_velocity.z = gyroZ imuMsg.angular_velocity_covariance = [ 0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01 ] # Accelerometer data (in meters per second squared): imuMsg.linear_acceleration.x = accelX imuMsg.linear_acceleration.y = accelY imuMsg.linear_acceleration.z = accelZ imuMsg.linear_acceleration_covariance = [ 0.01, 0.00, 0.00, 0.00, 0.01, 0.00, 0.00, 0.00, 0.01 ] imuPub.publish(imuMsg) if seq >= 300 and gyro_accuracy == 3 and cal_gyro == 1: # Stop dynamically calibrating the gyro to avoid unwanted drift rospy.loginfo("Stopping dynamic gyro calibration")
def handle_received_line(self, line): self._counter = self._counter + 1 self._SerialPublisher.publish(String(str(self._counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'b'): self._battery_value = float(lineParts[1]) ####################################################################################################################### self._battery_pub.publish(self._battery_value) ####################################################################################################################### if(lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) self._gx = float(lineParts[5]) self._gy = float(lineParts[6]) self._gz = float(lineParts[7]) self._ax = float(lineParts[8]) self._ay = float(lineParts[9]) self._az = float(lineParts[10]) ####################################################################################################################### imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self._frame_id imu_msg.header = h imu_msg.orientation_covariance = self._imu_data.orientation_covariance imu_msg.angular_velocity_covariance = self._imu_data.angular_velocity_covariance imu_msg.linear_acceleration_covariance = self._imu_data.linear_acceleration_covariance imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw imu_msg.angular_velocity.x = self._gx imu_msg.angular_velocity.y = self._gy imu_msg.angular_velocity.z = self._gz imu_msg.linear_acceleration.x = self._ax imu_msg.linear_acceleration.y = self._ay imu_msg.linear_acceleration.z = self._az #q_rot = quaternion_from_euler(self.pi, -self.pi/2, 0) #q_ori = Quaternion(self._qx, self._qy, self._qz, self._qw) #imu_msg.orientation = quaternion_multiply(q_ori, q_rot) self._imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
from math import atan; from math import atan2; from math import sqrt; import tf import os rospy.init_node("node") pub = rospy.Publisher('imu_data', Imu, queue_size=10) north_pub = rospy.Publisher('north', std_msgs.msg.Float32, queue_size=10) imuMsg = Imu() imuMsg.orientation_covariance = [999999 , 0 , 0, 0, 9999999, 0, 0, 0, 999999] imuMsg.angular_velocity_covariance = [9999, 0 , 0, 0 , 99999, 0, 0 , 0 , 0.02] imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0, 0 , 0.2, 0, 0 , 0 , 0.2] PI = 3.141592 grad2rad = PI/180.0 used_port='/dev/ttyACM0' #find the last ttyACM* #for possible_port in os.listdir('/dev/'): # if possible_port.startswith("ttyACM"): # used_port='/dev/'+possible_port
ax, ay, az = bno.read_linear_acceleration() # print x, y, z, w, vx, vy, vz, ax, ay, az quat = Quaternion(w, x, y, z) quat_norm = quat.normalised imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = "odom" imu_msg.orientation.w = w #quat_norm.elements[0] imu_msg.orientation.z = z #quat_norm.elements[1] imu_msg.orientation.y = y #quat_norm.elements[2] imu_msg.orientation.x = x #quat_norm.elements[3] imu_msg.orientation_covariance = [2, 0, 0, 0, 2, 0, 0, 0, 2] imu_msg.angular_velocity.x = vx imu_msg.angular_velocity.y = vy imu_msg.angular_velocity.z = vz imu_msg.angular_velocity_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1] imu_msg.linear_acceleration.x = ax imu_msg.linear_acceleration.y = ay imu_msg.linear_acceleration.z = az imu_msg.linear_acceleration_covariance = [1, 0, 0, 0, 1, 0, 0, 0, 1] pub.publish(imu_msg) # print('{0:0.2F} {1:0.2F} {2:0.2F} {3:0.2F} {4:0.2F} {5:0.2F} {6:0.2F} {7:0.2F} {8:0.2F} {9:0.2F}'.format( # x,y,z,w,vx,vy,vz,ax,ay,az)) time.sleep(0.1)
def talker(): QUEUE_SIZE = 10 RATE_PUBLISHER = 50 # Frequency in Hz SIZE_VECTOR = 3 MEAN_ACCEL = 0 MEAN_GYRO = 0 STD_DEVIATION_ACCEL = 0.2 STD_DEVIATION_GYRO = 0.1 INITIAL_YAW = 0 INITIAL_PITCH = 0 INITIAL_ROLL = 0 G = 9.81 # ROS Setup pub = rospy.Publisher('python_IMU', Imu, queue_size = QUEUE_SIZE) rospy.init_node('Python_IMU_Publisher_node') rate = rospy.Rate(RATE_PUBLISHER) while not rospy.is_shutdown(): print('Publishing new IMU Data') yaw = INITIAL_YAW pitch = INITIAL_PITCH roll = INITIAL_ROLL imuMsg = Imu() imuMsg.orientation_covariance = [0 , 0 , 0, 0, 0, 0, 0, 0, 0] imuMsg.angular_velocity_covariance = [0, 0 , 0, 0 , 0, 0, 0 , 0 , 0] imuMsg.linear_acceleration_covariance = [0 , 0 , 0, 0 , 0, 0, 0 , 0 , 0] # Noise is modelled as a gaussian distribution noise_accel = numpy.random.normal(MEAN_ACCEL, STD_DEVIATION_ACCEL, SIZE_VECTOR) noise_gyro = numpy.random.normal(MEAN_GYRO, STD_DEVIATION_GYRO, SIZE_VECTOR) # ------------------------------------------ # # Publish IMU data with added gaussian noise # ------------------------------------------ # # Accelerometer measurements imuMsg.linear_acceleration.x = 0 + noise_accel[0] imuMsg.linear_acceleration.y = 0 + noise_accel[1] imuMsg.linear_acceleration.z = G + noise_accel[2] # Gyroscope measurements imuMsg.angular_velocity.x = 0 + noise_gyro[0] imuMsg.angular_velocity.y = 0 + noise_gyro[1] imuMsg.angular_velocity.z = 0 + noise_gyro[2] # Quaternion imuMsg.orientation.x = 0 imuMsg.orientation.y = 0 imuMsg.orientation.z = 0 imuMsg.orientation.w = 1 # Time and frame name imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'base_link' # Publish and wait to achieve the correct frequency pub.publish(imuMsg) rate.sleep()
def compute_imu_msg(self): # Create new message try: # Get data self.get_imu_data() [ q1, q2, q3, accuracy, roll, pitch, yaw, w_x, w_y, w_z, acc_x, acc_y, acc_z ] = self.parse_msg() imu_msg = Imu() imu_msg.header.frame_id = self.tf_prefix + "imu" imu_msg.header.stamp = rospy.Time.now() # + rospy.Duration(0.5) if ((q1 * q1) + (q2 * q2) + (q3 * q3)) < 1: q4 = [ q1, q2, q3, np.sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))) ] else: self.log("Inconsistent readings from IMU", 2, alert="warn") return True # Compute the Orientation Quaternion new_q = Quaternion() new_q.x = q1 new_q.y = q2 new_q.z = q3 new_q.w = q4 imu_msg.orientation = new_q # Set the sensor covariances imu_msg.orientation_covariance = [ 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 ] # Angular Velocity imu_msg.angular_velocity.x = round(w_x, 4) imu_msg.angular_velocity.y = round(w_y, 4) imu_msg.angular_velocity.z = round(w_z, 4) # Datasheet says: # - Noise Spectral Density: 0.015dps/sqrt(Hz) # - Cross Axis Sensitivy: +-2% # diag = pow(0.015/np.sqrt(20), 2) # factor = 0.02 # imu_msg.angular_velocity_covariance = [ # diag, w_x*factor, w_x*factor, # w_y*factor, diag, w_y*factor, # w_z*factor, w_z*factor, diag # ] imu_msg.angular_velocity_covariance = [0.0] * 9 imu_msg.angular_velocity_covariance[0] = 0.0001 imu_msg.angular_velocity_covariance[4] = 0.0001 imu_msg.angular_velocity_covariance[8] = 0.0001 # imu_msg.angular_velocity_covariance = [-1] * 9 # Linear Acceleration imu_msg.linear_acceleration.x = round(acc_x, 4) imu_msg.linear_acceleration.y = round(acc_y, 4) imu_msg.linear_acceleration.z = round(acc_z, 4) # imu_msg.linear_acceleration.x = 0 # imu_msg.linear_acceleration.y = 0 # imu_msg.linear_acceleration.z = 9.82 # imu_msg.linear_acceleration_covariance = [-1] * 9 # Datasheet says: # - Noise Spectral Density: 230microg/sqrt(Hz) # - Cross Axis Sensitivy: +-2% # diag = pow(230e-6/np.sqrt(20), 2)/256. # factor = 0.02/256. # imu_msg.linear_acceleration_covariance = [ # diag, acc_x*factor, acc_x*factor, # acc_y*factor, diag, acc_y*factor, # acc_z*factor, acc_z*factor, diag # ] imu_msg.linear_acceleration_covariance = [0.0] * 9 imu_msg.linear_acceleration_covariance[0] = 0.001 imu_msg.linear_acceleration_covariance[4] = 0.001 imu_msg.linear_acceleration_covariance[8] = 0.001 # Message publishing self.imu_pub.publish(imu_msg) new_q = imu_msg.orientation [r, p, y] = transformations.euler_from_quaternion( [new_q.x, new_q.y, new_q.z, new_q.w]) self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format( r, p, y)) except SerialException as serial_exc: self.log( "SerialException while reading from IMU: {}".format( serial_exc), 3) self.calibration = True except ValueError as val_err: self.log("Value error from IMU data - {}".format(val_err), 5) self.val_exc = self.val_exc + 1 except Exception as imu_exc: self.log(imu_exc, 3) raise imu_exc
def _create_imu_msg(self,topic): """ Create imu message from ROV data Raises: Exception: No data available """ # Check if data is available if 'ATTITUDE' not in self.get_data(): raise Exception('no ATTITUDE data') #TODO: move all msgs creating to msg msg = Imu() self._create_header(msg) #http://mavlink.org/messages/common#SCALED_IMU imu_data = None for i in ['', '2', '3']: try: imu_data = self.get_data()['SCALED_IMU{}'.format(i)] break except Exception as e: pass if imu_data is None: raise Exception('no SCALED_IMUX data') acc_data = [imu_data['{}acc'.format(i)] for i in ['x', 'y', 'z']] gyr_data = [imu_data['{}gyro'.format(i)] for i in ['x', 'y', 'z']] mag_data = [imu_data['{}mag'.format(i)] for i in ['x', 'y', 'z']] #http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html msg.linear_acceleration.x = acc_data[0]/100 msg.linear_acceleration.y = acc_data[1]/100 msg.linear_acceleration.z = acc_data[2]/100 msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg.angular_velocity.x = gyr_data[0]/1000 msg.angular_velocity.y = gyr_data[1]/1000 msg.angular_velocity.z = gyr_data[2]/1000 msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] #http://mavlink.org/messages/common#ATTITUDE attitude_data = self.get_data()['ATTITUDE'] orientation = [attitude_data[i] for i in ['roll', 'pitch', 'yaw']] #https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_Angles_to_Quaternion_Conversion cy = math.cos(orientation[2] * 0.5) sy = math.sin(orientation[2] * 0.5) cr = math.cos(orientation[0] * 0.5) sr = math.sin(orientation[0] * 0.5) cp = math.cos(orientation[1] * 0.5) sp = math.sin(orientation[1] * 0.5) msg.orientation.w = cy * cr * cp + sy * sr * sp msg.orientation.x = cy * sr * cp - sy * cr * sp msg.orientation.y = cy * cr * sp + sy * sr * cp msg.orientation.z = sy * cr * cp - cy * sr * sp msg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] self.pub_topics[topic][3].publish(msg)
def spin_once(self): def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi * orient['roll'] / 180., pi * orient['pitch'] / 180, pi * orient['yaw'] / 180.) except KeyError: pass try: m = identity_matrix() m[:3, :3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp = data.get('Temp') # float raw_data = data.get('RAW') imu_data = data.get('Calib') orient_data = data.get('Orient') velocity_data = data.get('Vel') position_data = data.get('Pos') rawgps_data = data.get('RAWGPS') status = data.get('Stat') # int # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False # fill information where it's due # start by raw information that can be overriden if raw_data: # TODO warn about data not calibrated pub_imu = True pub_vel = True pub_mag = True pub_temp = True # acceleration imu_msg.linear_acceleration.x = raw_data['accX'] imu_msg.linear_acceleration.y = raw_data['accY'] imu_msg.linear_acceleration.z = raw_data['accZ'] imu_msg.linear_acceleration_covariance = (0., ) * 9 # gyroscopes imu_msg.angular_velocity.x = raw_data['gyrX'] imu_msg.angular_velocity.y = raw_data['gyrY'] imu_msg.angular_velocity.z = raw_data['gyrZ'] imu_msg.angular_velocity_covariance = (0., ) * 9 vel_msg.twist.angular.x = raw_data['gyrX'] vel_msg.twist.angular.y = raw_data['gyrY'] vel_msg.twist.angular.z = raw_data['gyrZ'] # magnetometer mag_msg.vector.x = raw_data['magX'] mag_msg.vector.y = raw_data['magY'] mag_msg.vector.z = raw_data['magZ'] # temperature # 2-complement decoding and 1/256 resolution x = raw_data['temp'] if x & 0x8000: temp_msg.data = (x - 1 << 16) / 256. else: temp_msg.data = x / 256. if rawgps_data: if rawgps_data['bGPS'] < self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT'] * 1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data[ 'LON'] * 1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT'] * 1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2 * rawgps_data['Hacc'] * 1e-3 xgps_msg.err_vert = 2 * rawgps_data['Vacc'] * 1e-3 self.old_bGPS = rawgps_data['bGPS'] if temp is not None: pub_temp = True temp_msg.data = temp if imu_data: try: x = imu_data['gyrX'] y = imu_data['gyrY'] z = imu_data['gyrZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.angular_velocity.x = v[0] imu_msg.angular_velocity.y = v[1] imu_msg.angular_velocity.z = v[2] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = v[0] vel_msg.twist.angular.y = v[1] vel_msg.twist.angular.z = v[2] pub_vel = True except KeyError: pass try: x = imu_data['accX'] y = imu_data['accY'] z = imu_data['accZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.linear_acceleration.x = v[0] imu_msg.linear_acceleration.y = v[1] imu_msg.linear_acceleration.z = v[2] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: x = imu_data['magX'] y = imu_data['magY'] z = imu_data['magZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) mag_msg.vector.x = v[0] mag_msg.vector.y = v[1] mag_msg.vector.z = v[2] pub_mag = True except KeyError: pass if velocity_data: pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] if orient_data: pub_imu = True orient_quat = quat_from_orient(orient_data) imu_msg.orientation.x = orient_quat[0] imu_msg.orientation.y = orient_quat[1] imu_msg.orientation.z = orient_quat[2] imu_msg.orientation.w = orient_quat[3] imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) if position_data: pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] if status is not None: if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) if pub_gps: if status & 0b0100: gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg)
def run(self): """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps. The IMU message, if fully filled in, contains information on orientation, acceleration (in m/s^2), and angular rate (in radians/sec). For each of these quantities, the IMU message format also wants the corresponding covariance matrix. Wiimote only gives us acceleration and angular rate. So we ensure that the orientation data entry is marked invalid. We do this by setting the first entry of its associated covariance matrix to -1. The covariance matrices are the 3x3 matrix with the axes' variance in the diagonal. We obtain the variance from the Wiimote instance. """ rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).") self.threadName = "IMU topic Publisher" try: while not rospy.is_shutdown(): (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData() msg = Imu( header=None, orientation=None, # will default to [0.,0.,0.,0], orientation_covariance=[ -1., 0., 0., 0., 0., 0., 0., 0., 0. ], # -1 indicates that orientation is unknown angular_velocity=None, angular_velocity_covariance=self. angular_velocity_covariance, linear_acceleration=None, linear_acceleration_covariance=self. linear_acceleration_covariance) # If a gyro is plugged into the Wiimote, then note the # angular velocity in the message, else indicate with # the special gyroAbsence_covariance matrix that angular # velocity is unavailable: if self.wiistate.motionPlusPresent: msg.angular_velocity.x = canonicalAngleRate[PHI] msg.angular_velocity.y = canonicalAngleRate[THETA] msg.angular_velocity.z = canonicalAngleRate[PSI] else: msg.angular_velocity_covariance = self.gyroAbsence_covariance msg.linear_acceleration.x = canonicalAccel[X] msg.linear_acceleration.y = canonicalAccel[Y] msg.linear_acceleration.z = canonicalAccel[Z] measureTime = self.wiistate.time timeSecs = int(measureTime) timeNSecs = int(abs(timeSecs - measureTime) * 10**9) msg.header.stamp.secs = timeSecs msg.header.stamp.nsecs = timeNSecs try: self.pub.publish(msg) except rospy.ROSException: rospy.loginfo( "Topic imu/data closed. Shutting down Imu sender.") exit(0) #rospy.logdebug("IMU state:") #rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate)) rospy.sleep(self.sleepDuration) except rospy.ROSInterruptException: rospy.loginfo("Shutdown request. Shutting down Imu sender.") exit(0)
def pub_imu_callback(self, event): """ This method is a callback of a timer. This publishes imu and pressure sensor data """ # TODO: euler rate is not angular velocity! # First publish pressure sensor msg = PressureSensor() msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'pressure_sensor' msg.temperature = 25.0 msg.pressure = self.odom.pose.pose.position.z * self.water_density * 9.81 / 100000.0 self.pub_pressure.publish(msg) # Imu imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = 'adis_imu' # Add some noise roll = self.orientation[0] + np.random.normal( 0.0, self.imu_orientation_covariance[0]) pitch = self.orientation[1] + np.random.normal( 0.0, self.imu_orientation_covariance[1]) yaw = self.orientation[2] + np.random.normal( 0.0, self.imu_orientation_covariance[2]) # Rotate as necessary vehicle_rpy = PyKDL.Rotation.RPY(roll, pitch, yaw) imu_orientation = self.imu_tf.M * vehicle_rpy # Get RPY angle = imu_orientation.GetRPY() # Derive to obtain rates if not self.imu_init: # Initialize heading buffer in order to apply a # savitzky_golay derivation if len(self.heading_buffer) == 0: self.heading_buffer.append(angle[2]) inc = cola2_lib.normalizeAngle(angle[2] - self.heading_buffer[-1]) self.heading_buffer.append(self.heading_buffer[-1] + inc) if len(self.heading_buffer) == len(self.savitzky_golay_coeffs): self.imu_init = True # Euler derivation to roll and pitch, so: self.last_imu_orientation = angle self.last_imu_update = imu.header.stamp else: period = (imu.header.stamp - self.last_imu_update).to_sec() if period < 0.001: period = 0.001 # Minimum allowed period # For yaw rate we apply a savitzky_golay derivation inc = cola2_lib.normalizeAngle(angle[2] - self.heading_buffer[-1]) self.heading_buffer.append(self.heading_buffer[-1] + inc) self.heading_buffer.pop(0) imu.angular_velocity.z = np.convolve(self.heading_buffer, self.savitzky_golay_coeffs, mode='valid') / period # TODO: Roll rate and Pitch rate should be also # savitzky_golay derivations? imu.angular_velocity.x = cola2_lib.normalizeAngle( angle[0] - self.last_imu_orientation[0]) / period imu.angular_velocity.y = cola2_lib.normalizeAngle( angle[1] - self.last_imu_orientation[1]) / period self.last_imu_orientation = angle self.last_imu_update = imu.header.stamp imu.angular_velocity_covariance = [ 0.0001, 0., 0., 0., 0.0001, 0., 0., 0., 0.0002 ] # Euler to Quaternion angle = tf.transformations.quaternion_from_euler( angle[0], angle[1], angle[2]) imu.orientation.x = angle[0] imu.orientation.y = angle[1] imu.orientation.z = angle[2] imu.orientation.w = angle[3] imu.orientation_covariance[0] = self.imu_orientation_covariance[0] imu.orientation_covariance[4] = self.imu_orientation_covariance[1] imu.orientation_covariance[8] = self.imu_orientation_covariance[2] #rospy.loginfo("%s: publishing imu", self.name) self.pub_imu.publish(imu) # Publish TF imu_tf = tf.TransformBroadcaster() o = tf.transformations.quaternion_from_euler( math.radians(self.imu_tf_array[3]), math.radians(self.imu_tf_array[4]), math.radians(self.imu_tf_array[5]), 'sxyz') imu_tf.sendTransform( (self.imu_tf_array[0], self.imu_tf_array[1], self.imu_tf_array[2]), o, imu.header.stamp, 'adis_imu_from_sim_nav_sensors', 'hug')
calib_data_print += line rospy.loginfo(calib_data_print) ''' roll = 0 pitch = 0 yaw = 0 seq = 0 rospy.loginfo("Giving the razor IMU board 3 seconds to boot...") rospy.sleep(3) # Sleep for 5 seconds to wait for the board to boot ### configure board ### # Orientation covariance estimation: imuMsg.orientation_covariance = ORIENT_COVARIANCE imuMsg.angular_velocity_covariance = ANGL_VEL_COVARIANCE imuMsg.linear_acceleration_covariance = LIN_ACCEL_COVARIANCE # ser.write(PAUSE_LOGGING) ser.write(DISABLE_TIME) ser.write(DISABLE_COMPASS) ser.write(ENABLE_EULER) ser.write(ENABLE_QUAT) # Read a sample and print print("Sample IMU Data:", ser.readline()) while not rospy.is_shutdown(): line = ser.readline() # IMU data: <timeMS>, <accelX>, <accelY>, <accelZ>, <gyroX>, <gyroY>, <gyroZ>, <magX>, <magY>, <magZ>
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z # Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss # Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could # cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians # i.e. variance in yaw: 0.0025 # Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause # static roll/pitch error of 0.8%, owing to gravity orientation sensing # error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025 # so set all covariances the same. imuMsg.orientation_covariance = [0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025] # Angular velocity covariance estimation: # Observed gyro noise: 4 counts => 0.28 degrees/sec # nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec # Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02 imuMsg.angular_velocity_covariance = [0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02] # linear acceleration covariance estimation: # observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2 # nonliniarity spec: 0.5% of full scale => 0.2m/s^2 # Choosing 0.2 as std dev, variance = 0.2^2 = 0.04 imuMsg.linear_acceleration_covariance = [0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04] # read basic informations port = rospy.get_param('~port', '/dev/ttyUSB0') topic_name = rospy.get_param('~topic', 'imu') link_name = rospy.get_param('~link_name', 'base_imu_link') # accelerometer accel_x_min = rospy.get_param('~accel_x_min', -250.0) accel_x_max = rospy.get_param('~accel_x_max', 250.0)
def talker(): global con pub_va = rospy.Publisher('imu/data_raw', Imu) pub_m = rospy.Publisher('imu/mag', Vector3Stamped) srv = rospy.Service('/tablet/command', StringSrv, handle_calls) rospy.init_node('imu') imu = Imu() mag = Vector3Stamped() last = "" imu.header.frame_id = mag.header.frame_id = "/tablet" v = math.pow(0.000028, 2) imu.angular_velocity_covariance = [v, 0, 0, 0, v, 0, 0, 0, v] imu.linear_acceleration_covariance = [v, 0, 0, 0, v, 0, 0, 0, v] imu.orientation_covariance = [v, 0, 0, 0, v, 0, 0, 0, v] while not rospy.is_shutdown(): s = last + con.read() last = "" up_imu = False up_mag = False for l in s.split("\n"): if l[-1:] != '|': last = l continue s = l[:-1].split(", ") try: for i in range(0, (len(s) - 1) / 4): t = int(s[i * 4 + 1]) x = float(s[i * 4 + 2]) y = float(s[i * 4 + 3]) z = float(s[i * 4 + 4]) if t == 3: imu.linear_acceleration.x = x imu.linear_acceleration.y = y imu.linear_acceleration.z = z up_imu = True elif t == 4: imu.angular_velocity.x = x imu.angular_velocity.y = y imu.angular_velocity.z = z up_imu = True elif t == 5: l = math.sqrt(x * x + y * y + z * z) mag.vector.x = x / l mag.vector.y = y / l mag.vector.z = z / l up_mag = True elif t == 6: qor = tf.transformations.quaternion_from_euler(x, y, z) imu.orientation.x = qor[0] imu.orientation.y = qor[1] imu.orientation.z = qor[2] imu.orientation.w = qor[3] except Exception as e: print e if up_imu: imu.header.stamp = rospy.Time.now() pub_va.publish(imu) if up_mag: mag.header.stamp = rospy.Time.now() pub_m.publish(mag) con.close()
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... # if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: # print 'no fix' # return # Changing from NED from the Applanix to ENU in ROS # Roll is still roll, since it's WRT to the x axis of the vehicle # Pitch is -ve since axis goes the other way (+y to right vs left) # Yaw (or heading) in Applanix is clockwise starting with North # In ROS it's counterclockwise startin with East time_stat = TimeSync() time_stat.ros_time = rospy.Time.now() time_stat.gps_time = data.td self.pub_time.publish(time_stat) t1 = time_stat.ros_time.secs + time_stat.ros_time.nsecs / 1E9 t2 = time_stat.gps_time.time1 # print '{0:6f}'.format(t1 - t2) orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch), RAD(90 - data.heading)).GetQuaternion() # UTM conversion utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude) # Initialize starting point if we haven't yet if not self.init and self.zero_start: self.origin.x = utm_pos.easting self.origin.y = utm_pos.northing self.origin.z = data.altitude self.init = True origin_param = { "east": self.origin.x, "north": self.origin.y, "alt": self.origin.z, } rospy.set_param('/gps_origin', origin_param) # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y p.position.z = self.origin.z self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = utm_pos.easting - self.origin.x odom.pose.pose.position.y = utm_pos.northing - self.origin.y odom.pose.pose.position.z = data.altitude - self.origin.z odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /reference frame or /vehicle frame and # NED to ENU conversion is needed here too odom.twist.twist.linear.x = data.east_vel odom.twist.twist.linear.y = data.north_vel odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion( *orient).z, Quaternion(*orient).w # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp, odom.child_frame_id, odom.header.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.z = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish( String(str(self._Counter) + ", in: " + line)) if (len(line) > 0): lineParts = line.split('\t') try: if (lineParts[0] == 'e'): self._left_encoder_value = long(lineParts[1]) self._right_encoder_value = long(lineParts[2]) ####################################################################################################################### self._Left_Encoder.publish(self._left_encoder_value) self._Right_Encoder.publish(self._right_encoder_value) ####################################################################################################################### if (lineParts[0] == 'b'): self._battery_value = float(lineParts[1]) ####################################################################################################################### self._Battery_Level.publish(self._battery_value) ####################################################################################################################### if (lineParts[0] == 's'): self._ultrasonic_value = float(lineParts[1]) ####################################################################################################################### self._Ultrasonic_Value.publish(self._ultrasonic_value) ####################################################################################################################### if (lineParts[0] == 'i'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) ####################################################################################################################### self._qx_.publish(self._qx) self._qy_.publish(self._qy) self._qz_.publish(self._qz) self._qw_.publish(self._qw) ####################################################################################################################### imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., ) * 9 imu_msg.angular_velocity_covariance = (-1., ) * 9 imu_msg.linear_acceleration_covariance = (-1., ) * 9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def get_sensor_data(self): """Read IMU data from the sensor, parse and publish.""" # Initialize ROS msgs imu_raw_msg = Imu() imu_msg = Imu() mag_msg = MagneticField() temp_msg = Temperature() # read from sensor buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45) # Publish raw data imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg() imu_raw_msg.header.frame_id = self.param.frame_id.value # TODO: do headers need sequence counters now? # imu_raw_msg.header.seq = seq # TODO: make this an option to publish? imu_raw_msg.orientation_covariance = [ self.param.variance_orientation.value[0], 0.0, 0.0, 0.0, self.param.variance_orientation.value[1], 0.0, 0.0, 0.0, self.param.variance_orientation.value[2] ] imu_raw_msg.linear_acceleration.x = \ self.unpackBytesToFloat(buf[0], buf[1]) / self.param.acc_factor.value imu_raw_msg.linear_acceleration.y = \ self.unpackBytesToFloat(buf[2], buf[3]) / self.param.acc_factor.value imu_raw_msg.linear_acceleration.z = \ self.unpackBytesToFloat(buf[4], buf[5]) / self.param.acc_factor.value imu_raw_msg.linear_acceleration_covariance = [ self.param.variance_acc.value[0], 0.0, 0.0, 0.0, self.param.variance_acc.value[1], 0.0, 0.0, 0.0, self.param.variance_acc.value[2] ] imu_raw_msg.angular_velocity.x = \ self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value imu_raw_msg.angular_velocity.y = \ self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value imu_raw_msg.angular_velocity.z = \ self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value imu_raw_msg.angular_velocity_covariance = [ self.param.variance_angular_vel.value[0], 0.0, 0.0, 0.0, self.param.variance_angular_vel.value[1], 0.0, 0.0, 0.0, self.param.variance_angular_vel.value[2] ] # node.get_logger().info('Publishing imu message') self.pub_imu_raw.publish(imu_raw_msg) # TODO: make this an option to publish? # Publish filtered data imu_msg.header.stamp = self.node.get_clock().now().to_msg() imu_msg.header.frame_id = self.param.frame_id.value q = Quaternion() # imu_msg.header.seq = seq q.w = self.unpackBytesToFloat(buf[24], buf[25]) q.x = self.unpackBytesToFloat(buf[26], buf[27]) q.y = self.unpackBytesToFloat(buf[28], buf[29]) q.z = self.unpackBytesToFloat(buf[30], buf[31]) # TODO(flynneva): replace with standard normalize() function # normalize norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) imu_msg.orientation.x = q.x / norm imu_msg.orientation.y = q.y / norm imu_msg.orientation.z = q.z / norm imu_msg.orientation.w = q.w / norm imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance imu_msg.linear_acceleration.x = \ self.unpackBytesToFloat(buf[32], buf[33]) / self.param.acc_factor.value imu_msg.linear_acceleration.y = \ self.unpackBytesToFloat(buf[34], buf[35]) / self.param.acc_factor.value imu_msg.linear_acceleration.z = \ self.unpackBytesToFloat(buf[36], buf[37]) / self.param.acc_factor.value imu_msg.linear_acceleration_covariance = imu_raw_msg.linear_acceleration_covariance imu_msg.angular_velocity.x = \ self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value imu_msg.angular_velocity.y = \ self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value imu_msg.angular_velocity.z = \ self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value imu_msg.angular_velocity_covariance = imu_raw_msg.angular_velocity_covariance self.pub_imu.publish(imu_msg) # Publish magnetometer data mag_msg.header.stamp = self.node.get_clock().now().to_msg() mag_msg.header.frame_id = self.param.frame_id.value # mag_msg.header.seq = seq mag_msg.magnetic_field.x = \ self.unpackBytesToFloat(buf[6], buf[7]) / self.param.mag_factor.value mag_msg.magnetic_field.y = \ self.unpackBytesToFloat(buf[8], buf[9]) / self.param.mag_factor.value mag_msg.magnetic_field.z = \ self.unpackBytesToFloat(buf[10], buf[11]) / self.param.mag_factor.value mag_msg.magnetic_field_covariance = [ self.param.variance_mag.value[0], 0.0, 0.0, 0.0, self.param.variance_mag.value[1], 0.0, 0.0, 0.0, self.param.variance_mag.value[2] ] self.pub_mag.publish(mag_msg) # Publish temperature temp_msg.header.stamp = self.node.get_clock().now().to_msg() temp_msg.header.frame_id = self.param.frame_id.value # temp_msg.header.seq = seq temp_msg.temperature = float(buf[44]) self.pub_temp.publish(temp_msg)