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