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 sensor_interface(): rospy.init_node('imu_razor9dof', anonymous = False) interface.flushInput() # Perform a loop checking on the enabled status of the drivers while not rospy.is_shutdown(): # Fetch a JSON message from the controller and process (or atleast attempt to) try: # Get the object and create some messages _object = json.loads(interface.readline()) _imu = Imu(); _heading = Float32() # Get the imu data (f**k quaternions!!) _imu.header = rospy.Header() roll = _object['angles'][0] pitch = _object['angles'][1] yaw = _object['angles'][2] #_imu.orientation = tf::createQuaternionFromRPY(roll, pitch, yaw) _imu.linear_acceleration.x = _object['accel'][0] _imu.linear_acceleration.y = _object['accel'][1] _imu.linear_acceleration.z = _object['accel'][2] # Get the heading data #_heading.data = _object['heading'] _heading.data = _object['angles'][2] # Publish the data imuPublisher.publish(_imu); cmpPublisher.publish(_heading); except json.decoder.JSONDecodeError: rospy.logwarn("Invalid message received") except: pass
def run(self): while not rospy.is_shutdown(): time_now = time.time() if time_now - self.timestamp > 0.5: self.cmd = (0, 0) self.lock.acquire() self.zumy.cmd(*self.cmd) imu_data = self.zumy.read_imu() self.lock.release() imu_msg = Imu() imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] self.imu_pub.publish(imu_msg) self.heartBeat.publish("I am alive from Glew") if self.msg != None: self.publisher.publish(self.msg.linear.y) self.rate.sleep() # If shutdown, turn off motors self.zumy.cmd(0, 0)
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(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 post_imu(self, component_instance): """ Publish the data of the IMU sensor as a custom ROS Imu message """ parent = component_instance.robot_parent parent_name = parent.blender_obj.name current_time = rospy.Time.now() imu = Imu() imu.header.seq = self._seq imu.header.stamp = current_time imu.header.frame_id = "/imu" imu.orientation = self.orientation.to_quaternion() imu.angular_velocity.x = component_instance.local_data['angular_velocity'][0] imu.angular_velocity.y = component_instance.local_data['angular_velocity'][1] imu.angular_velocity.z = component_instance.local_data['angular_velocity'][2] imu.linear_acceleration.x = component_instance.local_data['linear_acceleration'][0] imu.linear_acceleration.y = component_instance.local_data['linear_acceleration'][1] imu.linear_acceleration.z = component_instance.local_data['linear_acceleration'][2] for topic in self._topics: # publish the message on the correct w if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name): topic.publish(imu) self._seq += 1
def 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 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(self, event): compass_accel = self.compass_accel.read() compass = compass_accel[0:3] accel = compass_accel[3:6] gyro = self.gyro.read() # Put together an IMU message imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.orientation_covariance[0] = -1 imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD imu_msg.linear_acceleration.x = accel[0] * G imu_msg.linear_acceleration.y = accel[1] * G imu_msg.linear_acceleration.z = accel[2] * G self.pub_imu.publish(imu_msg) # Put together a magnetometer message mag_msg = MagneticField() mag_msg.header.stamp = rospy.Time.now() mag_msg.magnetic_field.x = compass[0] mag_msg.magnetic_field.y = compass[1] mag_msg.magnetic_field.z = compass[2] self.pub_mag.publish(mag_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 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 rospyrtimulib(): imuData = Imu() print("Using settings file " + SETTINGS_FILE + ".ini") if not os.path.exists(SETTINGS_FILE + ".ini"): print("Settings file does not exist, will be created") s = RTIMU.Settings(SETTINGS_FILE) imu = RTIMU.RTIMU(s) print("IMU Name: " + imu.IMUName()) if (not imu.IMUInit()): print("IMU Init Failed") sys.exit(1) else: print("IMU Init Succeeded") # this is a good time to set any fusion parameters imu.setSlerpPower(0.02) imu.setGyroEnable(True) imu.setAccelEnable(True) imu.setCompassEnable(True) pub = rospy.Publisher('imu', Imu, queue_size=10) rospy.init_node('rospyrtimulib', anonymous=True) rate = rospy.Rate(1000 / imu.IMUGetPollInterval()) while not rospy.is_shutdown(): if imu.IMURead(): # collect the data data = imu.getIMUData() fusionQPose = data["fusionQPose"] imuData.orientation.x = fusionQPose[1] imuData.orientation.y = fusionQPose[2] imuData.orientation.z = fusionQPose[3] imuData.orientation.w = fusionQPose[0] gyro = data["gyro"] imuData.angular_velocity.x = gyro[0] imuData.angular_velocity.y = gyro[1] imuData.angular_velocity.z = gyro[2] accel = data["accel"] imuData.linear_acceleration.x = accel[0] * 9.81 imuData.linear_acceleration.y = accel[1] * 9.81 imuData.linear_acceleration.z = accel[2] * 9.81 imuData.header.stamp = rospy.Time.now() # no covariance imuData.orientation_covariance[0] = -1 imuData.angular_velocity_covariance[0] = -1 imuData.linear_acceleration_covariance[0] = -1 imuData.header.frame_id = 'map' # publish it pub.publish(imuData) rate.sleep()
def talker(): pub = rospy.Publisher('imu', Imu, queue_size=10) rospy.init_node('imusensor', anonymous=True) r = rospy.Rate(500) while not rospy.is_shutdown(): #str = "hello world %s"%print_gyro().__str__() arr = print_gyro() i = Imu() i.header = std_msgs.msg.Header() i.header.frame_id="my_frame" i.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work i.linear_acceleration.x=arr[0][0] i.linear_acceleration.y=arr[0][1] i.linear_acceleration.z=arr[0][2] i.angular_velocity.x=arr[1][0] i.angular_velocity.y=arr[1][1] i.angular_velocity.z=arr[1][2] i.orientation.x=0 i.orientation.y=0 i.orientation.z=0 i.orientation.w=0 #rospy.loginfo(str) pub.publish(i) r.sleep()
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 run(self): while not rospy.is_shutdown(): self.lock.acquire() self.zumy.cmd(*self.cmd) imu_data = self.zumy.read_imu() enc_data = self.zumy.read_enc() self.lock.release() imu_msg = Imu() imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] self.imu_pub.publish(imu_msg) enc_msg = Int32() enc_msg.data = enc_data[0] self.r_enc_pub.publish(enc_msg) enc_msg.data = enc_data[1] self.l_enc_pub.publish(enc_msg) v_bat = self.zumy.read_voltage() self.batt_pub.publish(v_bat) self.heartBeat.publish("I am alive") self.rate.sleep() # If shutdown, turn off motors self.zumy.cmd(0,0)
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 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 _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 handleIMU(data): rospy.logdebug("TorsoIMU received for conversion: %f %f", data.angleX, data.angleY) imu_msg = Imu() q = tf.quaternion_from_euler(data.angleX, data.angleY, 0.0) imu_msg.header = data.header imu_msg.orientation.x = q[0] imu_msg.orientation.y = q[1] imu_msg.orientation.z = q[2] imu_msg.orientation.w = q[3] pub.publish(imu_msg)
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 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 send_message(self, event=None): if not self.valid or not self.connected: rospy.loginfo("%s: measurements not valid, not sending imu message!", self.name) return # NOTE: # the axis convention is taken from the old driver implementation # this should be checked again and explicitly stated in the documentation rotation = tft.quaternion_from_euler(0, 0, np.deg2rad(-self.yaw), axes="sxyz") # NOTE: # This is a message to hold data from an IMU (Inertial Measurement Unit) # # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec # # If the covariance of the measurement is known, it should be filled in (if all you know is the # variance of each measurement, e.g. from the datasheet, just put those along the diagonal) # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the # data a covariance will have to be assumed or gotten from some other source # # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation # estimate), please set element 0 of the associated covariance matrix to -1 # If you are interpreting this message, please check for a value of -1 in the first element of each # covariance matrix, and disregard the associated estimate. msg = Imu() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.frame_id msg.orientation.x = rotation[0] msg.orientation.y = rotation[1] msg.orientation.z = rotation[2] msg.orientation.w = rotation[3] # this is derived by looking at the datasheet under the bias offset factor # and it should include the drift of the gyro if the yaw is computed by integration msg.orientation_covariance[8] = np.deg2rad(self.yaw_std_z ** 2) # this is to notify that this imu is not measuring this data msg.linear_acceleration_covariance[0] = -1 msg.angular_velocity.z = np.deg2rad(-self.yaw_dot) # this is derived by looking at the datasheet msg.angular_velocity_covariance[8] = np.deg2rad(KVH_SIGMA ** 2) self.pub_gyro.publish(msg) if self.mode == KVH_MODE_R: rospy.loginfo("%s: raw data: %+.5f -- yaw rate: %+.5f", self.name, self.data, self.yaw_dot) else: rospy.loginfo( "%s: raw data: %+.5f -- yaw rate: %+.5f -- yaw: %+.5f", self.name, self.data, self.yaw_dot, self.yaw )
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 publish(self,event): compass_accel = self.compass_accel.read() compass = compass_accel[1] accel = compass_accel[0] gyro = self.gyro.read() # Put together an IMU message imu_msg = Imu() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = 'map' # covariance matrix imu_msg.orientation_covariance[0] = -1 imu_msg.angular_velocity_covariance[0] = -1 imu_msg.linear_acceleration_covariance[0] = -1 # angular velocity imu_msg.angular_velocity.x = gyro[0][0]*self.DEG2RAD imu_msg.angular_velocity.y = gyro[0][1]*self.DEG2RAD imu_msg.angular_velocity.z = gyro[0][2]*self.DEG2RAD # linear acceleration imu_msg.linear_acceleration.x = accel[0]*self.G imu_msg.linear_acceleration.y = accel[1]*self.G imu_msg.linear_acceleration.z = (accel[2])*self.G # calculate RPY # refers to Adafruit_9DOF.cpp in GitHub roll = math.atan2(accel[1], (accel[2])) pitch = math.atan2(-accel[0], (accel[1] * math.sin(roll) + accel[2] * math.cos(roll))) #yaw = math.atan2(compass[1],compass[0]) yaw = 0 #print "R=", math.degrees(roll)," P=", math.degrees(pitch), " Y=",math.degrees(yaw) # uncomment to print RPY # convert RPY to quaternion quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) # orientation imu_msg.orientation.x = quaternion[0] imu_msg.orientation.y = quaternion[1] imu_msg.orientation.z = quaternion[2] imu_msg.orientation.w = quaternion[3] # publish self.pub_imu.publish(imu_msg) # Put together a magnetometer message mag_msg = MagneticField() mag_msg.header.stamp = rospy.Time.now() mag_msg.magnetic_field.x = compass[0] mag_msg.magnetic_field.y = compass[1] mag_msg.magnetic_field.z = compass[2] # publish self.pub_mag.publish(mag_msg) RY_msg = Float64MultiArray() RY_msg.data = roll, pitch self.pub_RP.publish(RY_msg) '''self.br.sendTransform((0,0,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] == '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
continue strline = f1.readline() while strline: sli = strline.split(",") strline = f1.readline() if float(sli[0]) < float(sli0[0]): continue if float(sli[0]) == float(sli0[0]): print(sli[0]) w, x, y, z = EulerAndQuaternionTransform( [float(sli0[4]), float(sli0[5]), float(sli0[6])]) m_imu = Imu() m_imu.header.stamp = rospy.Time.from_sec(float(sli[0])) m_imu.linear_acceleration.x = float(sli[4]) m_imu.linear_acceleration.y = float(sli[5]) m_imu.linear_acceleration.z = float(sli[6]) m_imu.angular_velocity.x = float(sli[1]) m_imu.angular_velocity.y = float(sli[2]) m_imu.angular_velocity.z = float(sli[3]) m_imu.orientation.w = w m_imu.orientation.x = x m_imu.orientation.y = y m_imu.orientation.z = z outbag.write('/imu', m_imu, m_imu.header.stamp) break for topic, msg, t in rosbag.Bag(dst_dir + bag_name).read_messages():
def publish(self, data): if not self._calib and data.getImuMsgId() == PUB_ID: q = data.getOrientation() roll, pitch, yaw = euler_from_quaternion((q.y, q.z, q.w, q.x)) #array = quaternion_from_euler(yaw + (self._angle * pi / 180), roll, pitch) #array = quaternion_from_euler(yaw + (self._angle * pi / 180), -1 (roll - 180), -1 * pitch) array = quaternion_from_euler((yaw + (self._angle * pi / 180)), roll, -1 * pitch) 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 __init__(self): #Set up udp and tcp sockets self.host = rospy.get_param('/imu_node/host', '192.168.17.210') print("Host set to", self.host) self.port = 7778 self.UDP_IP = "0.0.0.0" #any interface that tries to connect on the laptop self.addr = (self.host, self.port) self.imu_sock = self.setup_TCP_sock() print("TCP socket setup") self.udp_sock = self.setup_UDP_sock() print("UDP socket setup") #set up ROS nodes and publisher rospy.init_node("razor_node") #We only care about the most recent measurement, i.e. queue_size=1 self.pub = rospy.Publisher('imu', Imu, queue_size=1) self.srv = Server( imuConfig, self.reconfig_callback) # define dynamic_reconfigure callback self.diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1) self.diag_pub_time = rospy.get_time() #Set up IMU message self.imuMsg = Imu() # Orientation covariance estimation: # 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. self.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 self.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 self.imuMsg.linear_acceleration_covariance = [ 0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04 ] #accelerometer self.accel_x_min = rospy.get_param('~accel_x_min', -250.0) self.accel_x_max = rospy.get_param('~accel_x_max', 250.0) self.accel_y_min = rospy.get_param('~accel_y_min', -250.0) self.accel_y_max = rospy.get_param('~accel_y_max', 250.0) self.accel_z_min = rospy.get_param('~accel_z_min', -250.0) self.accel_z_max = rospy.get_param('~accel_z_max', 250.0) # magnetometer self.magn_x_min = rospy.get_param('~magn_x_min', -600.0) self.magn_x_max = rospy.get_param('~magn_x_max', 600.0) self.magn_y_min = rospy.get_param('~magn_y_min', -600.0) self.magn_y_max = rospy.get_param('~magn_y_max', 600.0) self.magn_z_min = rospy.get_param('~magn_z_min', -600.0) self.magn_z_max = rospy.get_param('~magn_z_max', 600.0) self.calibration_magn_use_extended = rospy.get_param( '~calibration_magn_use_extended', False) self.magn_ellipsoid_center = rospy.get_param( '~self.magn_ellipsoid_center', [0, 0, 0]) self.magn_ellipsoid_transform = rospy.get_param( '~self.magn_ellipsoid_transform', [[0, 0, 0], [0, 0, 0], [0, 0, 0]]) self.imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0) # gyroscope self.gyro_average_offset_x = rospy.get_param('~gyro_average_offset_x', 0.0) self.gyro_average_offset_y = rospy.get_param('~gyro_average_offset_y', 0.0) self.gyro_average_offset_z = rospy.get_param('~gyro_average_offset_z', 0.0) #rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max) #rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max) #rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(self.magn_ellipsoid_center), str(self.magn_ellipsoid_transform[0][0])) #rospy.loginfo("%f %f %f", gyro_average_offset_x, gyro_average_offset_y, gyro_average_offset_z) self.roll = 0 self.pitch = 0 self.yaw = 0 self.seq = 0 self.accel_factor = 9.806 / 256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2. #stop datastream self.imu_sock.send('#o0' + '\n') self.imu_sock.send('#o1' + '\n') # To start display angle and sensor reading in text self.imu_sock.send('#ox' + '\n') rospy.loginfo("Writing calibration values to razor IMU board...") print "Calibrating accelerometer" #set calibration values self.imu_sock.sendall('#caxm' + str(self.accel_x_min) + chr(13)) self.imu_sock.sendall('#caxM' + str(self.accel_x_max) + chr(13)) self.imu_sock.sendall('#caym' + str(self.accel_y_min) + chr(13)) self.imu_sock.sendall('#cayM' + str(self.accel_y_max) + chr(13)) self.imu_sock.sendall('#cazm' + str(self.accel_z_min) + chr(13)) self.imu_sock.sendall('#cazM' + str(self.accel_z_max) + chr(13)) print "Calibrating magnetometer" #calibration values for magnetometer if (not self.calibration_magn_use_extended): self.imu_sock.sendall('#cmxm' + str(self.magn_x_min) + chr(13)) self.imu_sock.sendall('#cmxM' + str(self.magn_x_max) + chr(13)) self.imu_sock.sendall('#cmym' + str(self.magn_y_min) + chr(13)) self.imu_sock.sendall('#cmyM' + str(self.magn_y_max) + chr(13)) self.imu_sock.sendall('#cmzm' + str(self.magn_z_min) + chr(13)) self.imu_sock.sendall('#cmzM' + str(self.magn_z_max) + chr(13)) else: self.imu_sock.sendall('#ccx' + str(self.magn_ellipsoid_center[0]) + chr(13)) self.imu_sock.sendall('#ccy' + str(self.magn_ellipsoid_center[1]) + chr(13)) self.imu_sock.sendall('#ccz' + str(self.magn_ellipsoid_center[2]) + chr(13)) self.imu_sock.sendall('#ctxX' + str(self.magn_ellipsoid_transform[0][0]) + chr(13)) self.imu_sock.sendall('#ctxY' + str(self.magn_ellipsoid_transform[0][1]) + chr(13)) self.imu_sock.sendall('#ctxZ' + str(self.magn_ellipsoid_transform[0][2]) + chr(13)) self.imu_sock.sendall('#ctyX' + str(self.magn_ellipsoid_transform[1][0]) + chr(13)) self.imu_sock.sendall('#ctyY' + str(self.magn_ellipsoid_transform[1][1]) + chr(13)) self.imu_sock.sendall('#ctyZ' + str(self.magn_ellipsoid_transform[1][2]) + chr(13)) self.imu_sock.sendall('#ctzX' + str(self.magn_ellipsoid_transform[2][0]) + chr(13)) self.imu_sock.sendall('#ctzY' + str(self.magn_ellipsoid_transform[2][1]) + chr(13)) self.imu_sock.sendall('#ctzZ' + str(self.magn_ellipsoid_transform[2][2]) + chr(13)) print "Offsetting for gyro drift" #Compensating for offset of gyro-drift self.imu_sock.sendall('#cgx' + str(self.gyro_average_offset_x) + '\n') self.imu_sock.sendall('#cgy' + str(self.gyro_average_offset_y) + '\n') self.imu_sock.sendall('#cgz' + str(self.gyro_average_offset_z) + '\n') #print calibration values for verification by user self.imu_sock.sendall('#p' + '\n') calib_data = self.recv_all() calib_data_print = "Printing set calibration values:\r\n" for line in calib_data: calib_data_print += line rospy.loginfo(calib_data_print) self.imu_sock.send('#o0' + '\n') #start datastream self.imu_sock.send('#o1' + '\n') #output all angles: self.imu_sock.send('#ox' + '\n') #automatic flush - NOT WORKING #ser.flushInput() #discard old input, still in invalid format #flush manually, as above command is not working - it breaks the serial connection rospy.loginfo("Flushing first 100 IMU entries...") for x in range(0, 100): line = self.recv_all() print "entry", x, ":", line print "done flushing" # self.imu_sock.send('#ox' + '\n') rospy.loginfo("Publishing IMU data...")
#!/usr/bin/env python import rospy from sensor_msgs.msg import Imu from geometry_msgs.msg import Quaternion if __name__ == "__main__": try: rospy.init_node('imu_zero') # publish (0,0,0,1) pub_imu_zero = rospy.Publisher("capra/imu/zero", Imu, queue_size=10) rate = rospy.Rate(10) zero = Imu() zero.header.frame_id = "odom" zero.orientation = Quaternion(0, 0, 0, 1) while not rospy.is_shutdown(): pub_imu_zero.publish(zero) rate.sleep() except rospy.ROSInterruptException: pass
def control(): # sp = np.load("xy_sin.npy") state_sub = rospy.Subscriber("/mavros/state", State, state_cb, queue_size=10) rospy.wait_for_service('mavros/cmd/arming') arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) acutator_control_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10) mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose", PoseStamped, queue_size=10) imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10) local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, lp_cb, queue_size=10) local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, lv_cb, queue_size=10) act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub", ActuatorControl, act_cb, queue_size=10) rospy.init_node('control', anonymous=True) rate = rospy.Rate(50.0) # print("*"*80) while not rospy.is_shutdown() and not current_state.connected: rate.sleep() rospy.loginfo(current_state.connected) # print("#"*80) pose = PoseStamped() # mocap_pose = PoseStamped() offb_set_mode = SetModeRequest() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.Time.now() i = 0 act = ActuatorControl() flag1 = False flag2 = False prev_imu_data = Imu() prev_time = rospy.Time.now() # prev_x = 0 # prev_y = 0 # prev_z = 0 # prev_vx = 0 # prev_vy = 0 # prev_vz = 0 prev_omega_x = 0 prev_omega_y = 0 prev_omega_z = 0 prev_vx = 0 prev_vy = 0 prev_vz = 0 delta_t = 0.02 count = 0 theta = 0.0 # rospy.loginfo("Outside") while not rospy.is_shutdown(): if current_state.mode != "OFFBOARD" and ( rospy.Time.now() - last_request > rospy.Duration(5.0)): offb_set_mode_response = set_mode_client(offb_set_mode) if offb_set_mode_response.mode_sent: rospy.loginfo("Offboard enabled") flag1 = True last_request = rospy.Time.now() else: if current_state.armed == False: arm_cmd_response = arming_client(arm_cmd) if arm_cmd_response.success: rospy.loginfo("Vehicle armed") flag2 = True last_request = rospy.Time.now() # rospy.loginfo("Inside") curr_time = rospy.Time.now() if flag1 and flag2: x_f.append(float(local_position.pose.position.x)) y_f.append(float(local_position.pose.position.y)) z_f.append(float(local_position.pose.position.z)) vx_f.append(float(local_velocity.twist.linear.x)) vy_f.append(float(local_velocity.twist.linear.y)) vz_f.append(float(local_velocity.twist.linear.z)) # print(local_velocity.twist.linear.x) orientation = [ imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z ] (roll, pitch, yaw) = quaternion_to_euler_angle( imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z) r_f.append(float(mt.radians(roll))) p_f.append(float(mt.radians(pitch))) yaw_f.append(float(mt.radians(yaw))) sin_r_f.append(mt.sin(float(mt.radians(roll)))) sin_p_f.append(mt.sin(float(mt.radians(pitch)))) sin_y_f.append(mt.sin(float(mt.radians(yaw)))) cos_r_f.append(mt.cos(float(mt.radians(roll)))) cos_p_f.append(mt.cos(float(mt.radians(pitch)))) cos_y_f.append(mt.cos(float(mt.radians(yaw)))) rs_f.append(float(imu_data.angular_velocity.x)) ps_f.append(float(imu_data.angular_velocity.y)) ys_f.append(float(imu_data.angular_velocity.z)) ix.append(float(ixx)) iy.append(float(iyy)) iz.append(float(izz)) m.append(float(mass)) u0.append(act_controls.controls[0]) u1.append(act_controls.controls[1]) u2.append(act_controls.controls[2]) u3.append(act_controls.controls[3]) pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 3 pose.pose.orientation.z = mt.sin(theta * PI / 2) pose.pose.orientation.w = mt.cos(theta * PI / 2) (roll, pitch, yaw) = quaternion_to_euler_angle(pose.pose.orientation.w, 0, 0, pose.pose.orientation.z) x_des.append(0) y_des.append(0) z_des.append(3) sin_y_des.append(yaw) cos_yaw_des.append(yaw) w_mag.append(0) w_x.append(0) w_y.append(0) w_z.append(0) n_t = curr_time - prev_time #delta_t = float(n_t.nsecs) * 1e-9 a_x.append( (float(local_velocity.twist.linear.x) - prev_vx) / delta_t) a_y.append( (float(local_velocity.twist.linear.y) - prev_vy) / delta_t) a_z.append( (float(local_velocity.twist.linear.z) - prev_vz) / delta_t) ax_f.append(float(imu_data.linear_acceleration.x)) ay_f.append(float(imu_data.linear_acceleration.y)) az_f.append(float(imu_data.linear_acceleration.z)) prev_vx = float(local_velocity.twist.linear.x) prev_vy = float(local_velocity.twist.linear.y) prev_vz = float(local_velocity.twist.linear.z) aplha_x.append( (float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t) aplha_y.append( (float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t) aplha_z.append( (float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t) prev_omega_x = float(imu_data.angular_velocity.x) prev_omega_y = float(imu_data.angular_velocity.y) prev_omega_z = float(imu_data.angular_velocity.z) theta += 1.0 / 100 count += 1 local_pos_pub.publish(pose) if (count >= data_points): break prev_time = curr_time rate.sleep() nn1_yaw_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u3 ], ndmin=2).transpose() nn1_yaw_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose() nn2_yaw_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u0, u1, u2 ], ndmin=2).transpose() nn2_yaw_grd_truth = np.array([aplha_x, aplha_y, aplha_z], ndmin=2).transpose() print('nn1_yaw_input_state :', nn1_yaw_input_state.shape) print('nn1_yaw_grd_truth :', nn1_yaw_grd_truth.shape) print('nn2_yaw_input_state :', nn2_yaw_input_state.shape) print('nn2_yaw_grd_truth :', nn2_yaw_grd_truth.shape) np.save('nn1_yaw_input_state.npy', nn1_yaw_input_state) np.save('nn1_yaw_grd_truth.npy', nn1_yaw_grd_truth) np.save('nn2_yaw_input_state.npy', nn2_yaw_input_state) np.save('nn2_yaw_grd_truth.npy', nn2_yaw_grd_truth) s_yaw = np.array([ x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f ], ndmin=2).transpose() u_yaw = np.array([u0, u1, u2, u3], ndmin=2).transpose() a_yaw = np.array([ax_f, ay_f, az_f], ndmin=2).transpose() print('s_yaw :', s_yaw.shape) print('u_yaw :', u_yaw.shape) print('a_yaw :', a_yaw.shape) np.save('s_yaw.npy', s_yaw) np.save('u_yaw.npy', u_yaw) np.save('a_yaw.npy', a_yaw)
(config['yaw_calibration'])) #if imu_yaw_calibration != config('yaw_calibration'): imu_yaw_calibration = config['yaw_calibration'] rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration)) return config rospy.init_node("razor_node") #We only care about the most recent measurement, i.e. queue_size=1 pub = rospy.Publisher('imu', Imu, queue_size=1) srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1) diag_pub_time = rospy.get_time() imuMsg = Imu() # Orientation covariance estimation: # 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
publish_imu_topic = "/imu/data_raw" # For Magdwick filter convension imu_magdwick_topic = "/imu/data" velocity_command_topic = "/cmd_vel" # Po mereni smazat raw_imu_publish = "raw_data_imu" # Define static odometry information odom = Odometry() odom.header.frame_id = frame_id odom.child_frame_id = child_frame_id odom.pose.covariance = cov_pose odom.twist.covariance = cov_twist # Define static IMU information imu = Imu() imu.header.frame_id = frame_id imu.orientation_covariance = orientation_cov imu.angular_velocity_covariance = ang_vel_cov # Define static magnetic information mag = MagneticField() mag.header.frame_id = frame_id mag.magnetic_field_covariance = mag_field_cov # Define flags is_initialized = False is_calibrated = False theta_start = 0 # Calibration
#Magnetic = true + westerly declination , True= Magnetic - westerly declination #Magnetic = true - easterly declination , True= Magnetic + easterly declination #Digital compass heading offset in degree # D_Compass_offset = rospy.get_param('~offset',0.) D_Compass_declination = rospy.get_param( '~declination', -7.462777777777778) * (math.pi / 180.0) # By defaule IMU use Megnatic North as zero degree in Quaternion # If we want to use it directly with GPS-UTM x,y as Global Heading, East is our zero D_Compass_UseEastAsZero = rospy.get_param('~UseEastAsZero', True) # use this try to control miss sync in USB-serial, when it happends, must restart Checksum_error_limits = rospy.get_param('~Checksum_error_limits', 10.) checksum_error_counter = 0 imu_data = Imu() imu_data = Imu(header=rospy.Header(frame_id="SpartonCompassIMU")) #TODO find a right way to convert imu acceleration/angularvel./orientation accuracy to covariance 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 ] myStr1 = '\r\n\r\nprinttrigger 0 set drop\r\n' # this is with true north setting # myStr2='printmask gyrop_trigger accelp_trigger or quat_trigger or yawt_trigger or time_trigger or set drop\r\n' # this is output magnetic north myStr2 = 'printmask gyrop_trigger accelp_trigger or quat_trigger or time_trigger or set drop\r\n'
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() # Initialize constants acc_fact = 1000.0 mag_fact = 16.0 gyr_fact = 900.0 # read from sensor buf = self.con.receive(registers.ACCEL_DATA, 45) # Publish raw data # TODO: convert rcl Clock time to ros time? # imu_raw_msg.header.stamp = node.get_clock().now() 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[0] = -1 imu_raw_msg.linear_acceleration.x = float( struct.unpack('h', struct.pack('BB', buf[0], buf[1]))[0]) / acc_fact imu_raw_msg.linear_acceleration.y = float( struct.unpack('h', struct.pack('BB', buf[2], buf[3]))[0]) / acc_fact imu_raw_msg.linear_acceleration.z = float( struct.unpack('h', struct.pack('BB', buf[4], buf[5]))[0]) / acc_fact imu_raw_msg.linear_acceleration_covariance[0] = -1 imu_raw_msg.angular_velocity.x = float( struct.unpack('h', struct.pack('BB', buf[12], buf[13]))[0]) / gyr_fact imu_raw_msg.angular_velocity.y = float( struct.unpack('h', struct.pack('BB', buf[14], buf[15]))[0]) / gyr_fact imu_raw_msg.angular_velocity.z = float( struct.unpack('h', struct.pack('BB', buf[16], buf[17]))[0]) / gyr_fact imu_raw_msg.angular_velocity_covariance[0] = -1 # 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 = node.get_clock().now() imu_msg.header.frame_id = self.param.frame_id.value q = Quaternion() # imu_msg.header.seq = seq q.w = float(struct.unpack('h', struct.pack('BB', buf[24], buf[25]))[0]) q.x = float(struct.unpack('h', struct.pack('BB', buf[26], buf[27]))[0]) q.y = float(struct.unpack('h', struct.pack('BB', buf[28], buf[29]))[0]) q.z = float(struct.unpack('h', struct.pack('BB', buf[30], buf[31]))[0]) # 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.linear_acceleration.x = float( struct.unpack('h', struct.pack('BB', buf[32], buf[33]))[0]) / acc_fact imu_msg.linear_acceleration.y = float( struct.unpack('h', struct.pack('BB', buf[34], buf[35]))[0]) / acc_fact imu_msg.linear_acceleration.z = float( struct.unpack('h', struct.pack('BB', buf[36], buf[37]))[0]) / acc_fact imu_msg.linear_acceleration_covariance[0] = -1 imu_msg.angular_velocity.x = float( struct.unpack('h', struct.pack('BB', buf[12], buf[13]))[0]) / gyr_fact imu_msg.angular_velocity.y = float( struct.unpack('h', struct.pack('BB', buf[14], buf[15]))[0]) / gyr_fact imu_msg.angular_velocity.z = float( struct.unpack('h', struct.pack('BB', buf[16], buf[17]))[0]) / gyr_fact imu_msg.angular_velocity_covariance[0] = -1 self.pub_imu.publish(imu_msg) # Publish magnetometer data # mag_msg.header.stamp = node.get_clock().now() mag_msg.header.frame_id = self.param.frame_id.value # mag_msg.header.seq = seq mag_msg.magnetic_field.x = \ float(struct.unpack('h', struct.pack('BB', buf[6], buf[7]))[0]) / mag_fact mag_msg.magnetic_field.y = \ float(struct.unpack('h', struct.pack('BB', buf[8], buf[9]))[0]) / mag_fact mag_msg.magnetic_field.z = \ float(struct.unpack('h', struct.pack('BB', buf[10], buf[11]))[0]) / mag_fact self.pub_mag.publish(mag_msg) # Publish temperature # temp_msg.header.stamp = node.get_clock().now() 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)
# Print out an error if system status is in error mode. if status == 0x01: rospy.logerr('System error: {0}'.format(error)) rospy.logerr('See datasheet section 4.3.59 for the meaning.') # Print BNO055 software revision and other diagnostic data. sw, bl, accel, mag, gyro = bno.get_revision() rospy.loginfo('Software version: {0}'.format(sw)) rospy.loginfo('Bootloader version: {0}'.format(bl)) rospy.loginfo('Accelerometer ID: 0x{0:02X}'.format(accel)) rospy.loginfo('Magnetometer ID: 0x{0:02X}'.format(mag)) rospy.loginfo('Gyroscope ID: 0x{0:02X}\n'.format(gyro)) print('Reading BNO055 data, press Ctrl-C to quit...') i = Imu() ps = PoseStamped() i.header.frame_id = 'BNO055' try: while True: ''' if confMode == False and (sys != 3 or mag != 3): print("Reloading calibration file...") bno.set_calibration(data) ''' # Read the Euler angles for heading, roll, pitch (all in degrees) heading, roll, pitch = bno.read_euler() # Read the calibration status, 0=uncalibrated and 3=fully calibrated sys, gyro, accel, mag = bno.get_calibration_status()
print('Roll={0:0.8F} Pitch={1:0.8F} Yaw={2:0.8F}'.format(roll, pitch, yaw)) def status_callback(imu_status): system_status, gyro_status, accel_status, mag_status = imu_status.data print('Status: Sys={0} Mag={1} Accel={2} Gyro={3}'.format(system_status, mag_status, accel_status, gyro_status)) def accuracy_callback(accuracy): print('Rotation Accuracy={0}'.format(accuracy.data)) # odomMsg = Odometry() # rospy.Subscriber("/odometry/filtered", Odometry, odom_callback) imuMsg = Imu() rospy.Subscriber("/imu/data", Imu, imu_callback) # statusMessage = Int8MultiArray() # rospy.Subscriber("/imu/debug", Int8MultiArray, status_callback) accuracyMessage = Int8MultiArray() rospy.Subscriber("/imu/direction_accuracy", Float32, accuracy_callback) def update_position(): rate.sleep() while not rospy.is_shutdown():
def talker(): global global_g, deg_rad ser.flush() pub = rospy.Publisher('/imu/data_raw', Imu, queue_size=100) rospy.init_node('imu_node', anonymous=True) rate = rospy.Rate(100) # 10hz imuMsg = Imu() seq = 0 while not rospy.is_shutdown(): #$GTIMU,GPSWeek,GPSTime,GyroX,GyroY,GyroZ,AccX,AccY,AccZ,Tpr*cs<CR><LF> if ser.read() != '$': continue line0 = ser.readline() print line0 cs = line0[-4:-2] print cs cs1 = int(cs, 16) cs2 = checksum(line0) print("cs1,cs2", cs1, cs2) if cs1 != cs2: continue if line0[0:5] == "GTIMU": line = line0.replace("GTIMU,", "") line = line.replace("\r\n", "") if "\x00" in line: continue if not string.find('*', line): continue line = line.replace("*", ",") words = string.split(line, ",") GyroX = string.atof(words[2]) GyroY = string.atof(words[3]) GyroZ = string.atof(words[4]) AccX = string.atof(words[5]) AccY = string.atof(words[6]) AccZ = string.atof(words[7]) Tpr = string.atof(words[8]) imuMsg.linear_acceleration.x = AccX * global_g imuMsg.linear_acceleration.y = AccY * global_g imuMsg.linear_acceleration.z = AccZ * global_g imuMsg.linear_acceleration_covariance[0] = 0.0001 imuMsg.linear_acceleration_covariance[4] = 0.0001 imuMsg.linear_acceleration_covariance[8] = 0.0001 imuMsg.angular_velocity.x = GyroX * deg_rad imuMsg.angular_velocity.y = GyroY * deg_rad imuMsg.angular_velocity.z = GyroZ * deg_rad imuMsg.angular_velocity_covariance[0] = 0.0001 imuMsg.angular_velocity_covariance[4] = 0.0001 imuMsg.angular_velocity_covariance[8] = 0.0001 q = quaternion_from_euler(0, 0, 0) imuMsg.orientation.x = q[0] imuMsg.orientation.y = q[1] imuMsg.orientation.z = q[2] imuMsg.orientation.w = q[3] imuMsg.header.stamp = rospy.Time.now() imuMsg.header.frame_id = 'imu' imuMsg.header.seq = seq seq = seq + 1 pub.publish(imuMsg) rate.sleep() else: continue
def update_sensors(self): # print "accelerometer:", self._bridge.get_accelerometer() # print "proximity:", self._bridge.get_proximity() # print "light:", self._bridge.get_light_sensor() # print "motor_position:", self._bridge.get_motor_position() # print "floor:", self._bridge.get_floor_sensors() # print "image:", self._bridge.get_image() ## If image from camera if self.enabled_sensors['camera']: # Get Image image = self._bridge.get_image() #print image if image is not None: nimage = np.asarray(image) image_msg = CvBridge().cv2_to_imgmsg(nimage, "rgb8") self.image_publisher.publish(image_msg) if self.enabled_sensors['proximity']: prox_sensors = self._bridge.get_proximity() for i in range(0, 8): if prox_sensors[i] > 0: self.prox_msg[i].range = 0.5 / math.sqrt( prox_sensors[i] ) # Transform the analog value to a distance value in meters (given from field tests). else: self.prox_msg[i].range = self.prox_msg[i].max_range if self.prox_msg[i].range > self.prox_msg[i].max_range: self.prox_msg[i].range = self.prox_msg[i].max_range if self.prox_msg[i].range < self.prox_msg[i].min_range: self.prox_msg[i].range = self.prox_msg[i].min_range self.prox_msg[i].header.stamp = rospy.Time.now() self.prox_publisher[i].publish(self.prox_msg[i]) # e-puck proximity positions (cm), x pointing forward, y pointing left # P7(3.5, 1.0) P0(3.5, -1.0) # P6(2.5, 2.5) P1(2.5, -2.5) # P5(0.0, 3.0) P2(0.0, -3.0) # P4(-3.5, 2.0) P3(-3.5, -2.0) # # e-puck proximity orentations (degrees) # P7(10) P0(350) # P6(40) P1(320) # P5(90) P2(270) # P4(160) P3(200) self.br.sendTransform( (0.035, -0.010, 0.034), tf.transformations.quaternion_from_euler(0, 0, 6.11), rospy.Time.now(), self._name + "/base_prox0", self._name + "/base_link") self.br.sendTransform( (0.025, -0.025, 0.034), tf.transformations.quaternion_from_euler(0, 0, 5.59), rospy.Time.now(), self._name + "/base_prox1", self._name + "/base_link") self.br.sendTransform( (0.000, -0.030, 0.034), tf.transformations.quaternion_from_euler(0, 0, 4.71), rospy.Time.now(), self._name + "/base_prox2", self._name + "/base_link") self.br.sendTransform( (-0.035, -0.020, 0.034), tf.transformations.quaternion_from_euler(0, 0, 3.49), rospy.Time.now(), self._name + "/base_prox3", self._name + "/base_link") self.br.sendTransform( (-0.035, 0.020, 0.034), tf.transformations.quaternion_from_euler(0, 0, 2.8), rospy.Time.now(), self._name + "/base_prox4", self._name + "/base_link") self.br.sendTransform( (0.000, 0.030, 0.034), tf.transformations.quaternion_from_euler(0, 0, 1.57), rospy.Time.now(), self._name + "/base_prox5", self._name + "/base_link") self.br.sendTransform( (0.025, 0.025, 0.034), tf.transformations.quaternion_from_euler(0, 0, 0.70), rospy.Time.now(), self._name + "/base_prox6", self._name + "/base_link") self.br.sendTransform( (0.035, 0.010, 0.034), tf.transformations.quaternion_from_euler(0, 0, 0.17), rospy.Time.now(), self._name + "/base_prox7", self._name + "/base_link") if self.enabled_sensors['accelerometer']: accel = self._bridge.get_accelerometer() accel_msg = Imu() accel_msg.header.stamp = rospy.Time.now() accel_msg.header.frame_id = self._name + "/base_link" accel_msg.linear_acceleration.x = ( accel[1] - 2048.0 ) / 800.0 * 9.81 # 1 g = about 800, then transforms in m/s^2. accel_msg.linear_acceleration.y = (accel[0] - 2048.0) / 800.0 * 9.81 accel_msg.linear_acceleration.z = (accel[2] - 2048.0) / 800.0 * 9.81 accel_msg.linear_acceleration_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] #print "accel raw: " + str(accel[0]) + ", " + str(accel[1]) + ", " + str(accel[2]) #print "accel (m/s2): " + str((accel[0]-2048.0)/800.0*9.81) + ", " + str((accel[1]-2048.0)/800.0*9.81) + ", " + str((accel[2]-2048.0)/800.0*9.81) accel_msg.angular_velocity.x = 0 accel_msg.angular_velocity.y = 0 accel_msg.angular_velocity.z = 0 accel_msg.angular_velocity_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] q = tf.transformations.quaternion_from_euler(0, 0, 0) accel_msg.orientation = Quaternion(*q) accel_msg.orientation_covariance = [ 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01 ] self.accel_publisher.publish(accel_msg) if self.enabled_sensors['motor_position']: motor_pos = list( self._bridge.get_motor_position() ) # Get a list since tuple elements returned by the function are immutable. # Convert to 16 bits signed integer. if (motor_pos[0] & 0x8000): motor_pos[0] = -0x10000 + motor_pos[0] if (motor_pos[1] & 0x8000): motor_pos[1] = -0x10000 + motor_pos[1] #print "motor_pos: " + str(motor_pos[0]) + ", " + str(motor_pos[1]) self.leftStepsDiff = motor_pos[ 0] * MOT_STEP_DIST - self.leftStepsPrev # Expressed in meters. self.rightStepsDiff = motor_pos[ 1] * MOT_STEP_DIST - self.rightStepsPrev # Expressed in meters. #print "left, right steps diff: " + str(self.leftStepsDiff) + ", " + str(self.rightStepsDiff) self.deltaTheta = (self.rightStepsDiff - self.leftStepsDiff ) / WHEEL_DISTANCE # Expressed in radiant. self.deltaSteps = (self.rightStepsDiff + self.leftStepsDiff) / 2 # Expressed in meters. #print "delta theta, steps: " + str(self.deltaTheta) + ", " + str(self.deltaSteps) self.x_pos += self.deltaSteps * math.cos( self.theta + self.deltaTheta / 2) # Expressed in meters. self.y_pos += self.deltaSteps * math.sin( self.theta + self.deltaTheta / 2) # Expressed in meters. self.theta += self.deltaTheta # Expressed in radiant. #print "x, y, theta: " + str(self.x_pos) + ", " + str(self.y_pos) + ", " + str(self.theta) self.leftStepsPrev = motor_pos[ 0] * MOT_STEP_DIST # Expressed in meters. self.rightStepsPrev = motor_pos[ 1] * MOT_STEP_DIST # Expressed in meters. odom_msg = Odometry() odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = "odom" odom_msg.child_frame_id = self._name + "/base_link" odom_msg.pose.pose.position = Point(self.x_pos, self.y_pos, 0) q = tf.transformations.quaternion_from_euler(0, 0, self.theta) odom_msg.pose.pose.orientation = Quaternion(*q) self.endTime = time.time() odom_msg.twist.twist.linear.x = self.deltaSteps / ( self.endTime - self.startTime ) # self.deltaSteps is the linear distance covered in meters from the last update (delta distance); # the time from the last update is measured in seconds thus to get m/s we multiply them. odom_msg.twist.twist.angular.z = self.deltaTheta / ( self.endTime - self.startTime ) # self.deltaTheta is the angular distance covered in radiant from the last update (delta angle); # the time from the last update is measured in seconds thus to get rad/s we multiply them. #print "time elapsed = " + str(self.endTime-self.startTime) + " seconds" self.startTime = self.endTime self.odom_publisher.publish(odom_msg) pos = (odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z) ori = (odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z, odom_msg.pose.pose.orientation.w) self.br.sendTransform(pos, ori, odom_msg.header.stamp, odom_msg.child_frame_id, odom_msg.header.frame_id) if self.enabled_sensors['light']: light_sensors = self._bridge.get_light_sensor() light_sensors_marker_msg = Marker() light_sensors_marker_msg.header.frame_id = self._name + "/base_link" light_sensors_marker_msg.header.stamp = rospy.Time.now() light_sensors_marker_msg.type = Marker.TEXT_VIEW_FACING light_sensors_marker_msg.pose.position.x = 0.15 light_sensors_marker_msg.pose.position.y = 0 light_sensors_marker_msg.pose.position.z = 0.15 q = tf.transformations.quaternion_from_euler(0, 0, 0) light_sensors_marker_msg.pose.orientation = Quaternion(*q) light_sensors_marker_msg.scale.z = 0.01 light_sensors_marker_msg.color.a = 1.0 light_sensors_marker_msg.color.r = 1.0 light_sensors_marker_msg.color.g = 1.0 light_sensors_marker_msg.color.b = 1.0 light_sensors_marker_msg.text = "light: " + str(light_sensors) self.light_publisher.publish(light_sensors_marker_msg) if self.enabled_sensors['floor']: floor_sensors = self._bridge.get_floor_sensors() floor_marker_msg = Marker() floor_marker_msg.header.frame_id = self._name + "/base_link" floor_marker_msg.header.stamp = rospy.Time.now() floor_marker_msg.type = Marker.TEXT_VIEW_FACING floor_marker_msg.pose.position.x = 0.15 floor_marker_msg.pose.position.y = 0 floor_marker_msg.pose.position.z = 0.13 q = tf.transformations.quaternion_from_euler(0, 0, 0) floor_marker_msg.pose.orientation = Quaternion(*q) floor_marker_msg.scale.z = 0.01 floor_marker_msg.color.a = 1.0 floor_marker_msg.color.r = 1.0 floor_marker_msg.color.g = 1.0 floor_marker_msg.color.b = 1.0 floor_marker_msg.text = "floor: " + str(floor_sensors) self.floor_publisher.publish(floor_marker_msg) if self.enabled_sensors['selector']: curr_sel = self._bridge.get_selector() selector_marker_msg = Marker() selector_marker_msg.header.frame_id = self._name + "/base_link" selector_marker_msg.header.stamp = rospy.Time.now() selector_marker_msg.type = Marker.TEXT_VIEW_FACING selector_marker_msg.pose.position.x = 0.15 selector_marker_msg.pose.position.y = 0 selector_marker_msg.pose.position.z = 0.11 q = tf.transformations.quaternion_from_euler(0, 0, 0) selector_marker_msg.pose.orientation = Quaternion(*q) selector_marker_msg.scale.z = 0.01 selector_marker_msg.color.a = 1.0 selector_marker_msg.color.r = 1.0 selector_marker_msg.color.g = 1.0 selector_marker_msg.color.b = 1.0 selector_marker_msg.text = "selector: " + str(curr_sel) self.selector_publisher.publish(selector_marker_msg) if self.enabled_sensors['motor_speed']: motor_speed = list( self._bridge.get_motor_speed() ) # Get a list since tuple elements returned by the function are immutable. # Convert to 16 bits signed integer. if (motor_speed[0] & 0x8000): motor_speed[0] = -0x10000 + motor_speed[0] if (motor_speed[1] & 0x8000): motor_speed[1] = -0x10000 + motor_speed[1] motor_speed_marker_msg = Marker() motor_speed_marker_msg.header.frame_id = self._name + "/base_link" motor_speed_marker_msg.header.stamp = rospy.Time.now() motor_speed_marker_msg.type = Marker.TEXT_VIEW_FACING motor_speed_marker_msg.pose.position.x = 0.15 motor_speed_marker_msg.pose.position.y = 0 motor_speed_marker_msg.pose.position.z = 0.09 q = tf.transformations.quaternion_from_euler(0, 0, 0) motor_speed_marker_msg.pose.orientation = Quaternion(*q) motor_speed_marker_msg.scale.z = 0.01 motor_speed_marker_msg.color.a = 1.0 motor_speed_marker_msg.color.r = 1.0 motor_speed_marker_msg.color.g = 1.0 motor_speed_marker_msg.color.b = 1.0 motor_speed_marker_msg.text = "speed: " + str(motor_speed) self.motor_speed_publisher.publish(motor_speed_marker_msg) if self.enabled_sensors['microphone']: mic = self._bridge.get_microphone() microphone_marker_msg = Marker() microphone_marker_msg.header.frame_id = self._name + "/base_link" microphone_marker_msg.header.stamp = rospy.Time.now() microphone_marker_msg.type = Marker.TEXT_VIEW_FACING microphone_marker_msg.pose.position.x = 0.15 microphone_marker_msg.pose.position.y = 0 microphone_marker_msg.pose.position.z = 0.07 q = tf.transformations.quaternion_from_euler(0, 0, 0) microphone_marker_msg.pose.orientation = Quaternion(*q) microphone_marker_msg.scale.z = 0.01 microphone_marker_msg.color.a = 1.0 microphone_marker_msg.color.r = 1.0 microphone_marker_msg.color.g = 1.0 microphone_marker_msg.color.b = 1.0 microphone_marker_msg.text = "microphone: " + str(mic) self.microphone_publisher.publish(microphone_marker_msg)
def main(): # init imu node rospy.init_node('imu', anonymous=False) # get imu config imu_manager = imu.IMU(rospy.get_param('/ema/imu')) # list published topics pub = {} for name in imu_manager.imus: # pub[name] = rospy.Publisher('imu/' + name, String, queue_size=10) pub[name] = rospy.Publisher('imu/' + name, Imu, queue_size=10) # pub[name + '_buttons'] = rospy.Publisher('imu/' + name + '_buttons', Int8, queue_size=10) # define loop rate (in hz) rate = rospy.Rate(200) # node loop while not rospy.is_shutdown(): try: timestamp = rospy.Time.now() frame_id = 'base_link' # POOLING MODE if imu_manager.streaming == False: ## messages are shared by all imus imuMsg = Imu() imuMsg.header.stamp = timestamp imuMsg.header.frame_id = frame_id buttons = Int8() for name in imu_manager.imus: orientation = imu_manager.getQuaternion(name) imuMsg.orientation.x = orientation[0] imuMsg.orientation.y = orientation[1] imuMsg.orientation.z = orientation[2] imuMsg.orientation.w = orientation[3] angular_velocity = imu_manager.getGyroData(name) imuMsg.angular_velocity.x = angular_velocity[0] imuMsg.angular_velocity.y = angular_velocity[1] imuMsg.angular_velocity.z = angular_velocity[2] linear_acceleration = imu_manager.getAccelData(name) imuMsg.linear_acceleration.x = -linear_acceleration[0] imuMsg.linear_acceleration.y = -linear_acceleration[1] imuMsg.linear_acceleration.z = -linear_acceleration[2] pub[name].publish(imuMsg) buttons = imu_manager.getButtonState(name) pub[name + '_buttons'].publish(buttons) else: # STREAMING MODE if imu_manager.broadcast == False: for name in imu_manager.imus: # one message per imu imuMsg = Imu() imuMsg.header.stamp = timestamp imuMsg.header.frame_id = frame_id # buttons = Int8() streaming_data = imu_manager.getStreamingData(name) idx = 0 for slot in imu_manager.streaming_slots[name]: #print name, slot if slot == 'getTaredOrientationAsQuaternion': imuMsg.orientation.x = streaming_data[idx] imuMsg.orientation.y = streaming_data[idx + 1] imuMsg.orientation.z = streaming_data[idx + 2] imuMsg.orientation.w = streaming_data[idx + 3] idx = idx + 4 elif slot == 'getNormalizedGyroRate': imuMsg.angular_velocity.x = streaming_data[idx] imuMsg.angular_velocity.y = streaming_data[idx + 1] imuMsg.angular_velocity.z = streaming_data[idx + 2] idx = idx + 3 elif slot == 'getCorrectedAccelerometerVector': imuMsg.linear_acceleration.x = -streaming_data[ idx] imuMsg.linear_acceleration.y = -streaming_data[ idx + 1] imuMsg.linear_acceleration.z = -streaming_data[ idx + 2] idx = idx + 3 print type(streaming_data) elif slot == 'getButtonState': if type(streaming_data) == 'tuple': buttons = streaming_data[idx] idx = idx + 1 else: # imu is only streaming button state, result is not a tuple buttons = streaming_data # publish streamed data pub[name].publish(imuMsg) # pub[name + '_buttons'].publish(buttons) # BROADCAST MODE else: for name in imu_manager.imus: ## one message per imu imuMsg = Imu() imuMsg.header.stamp = timestamp imuMsg.header.frame_id = frame_id buttons = Int8() streaming_data = imu_manager.devices[ name].getStreamingBatch() idx = 0 imuMsg.orientation.x = streaming_data[idx] imuMsg.orientation.y = streaming_data[idx + 1] imuMsg.orientation.z = streaming_data[idx + 2] imuMsg.orientation.w = streaming_data[idx + 3] idx = idx + 4 imuMsg.angular_velocity.x = streaming_data[idx] imuMsg.angular_velocity.y = streaming_data[idx + 1] imuMsg.angular_velocity.z = streaming_data[idx + 2] idx = idx + 3 imuMsg.linear_acceleration.x = -streaming_data[idx] imuMsg.linear_acceleration.y = -streaming_data[idx + 1] imuMsg.linear_acceleration.z = -streaming_data[idx + 2] idx = idx + 3 buttons = streaming_data[idx] # publish streamed data pub[name].publish(imuMsg) pub[name + '_buttons'].publish(buttons) except TypeError: print 'TypeError occured!' # sleep until it's time to work again rate.sleep() # cleanup imu_manager.shutdown()
GPS[i] = False mx = new_acc_imu[0, :] # acceleration x axis my = new_acc_imu[1, :] mz = new_acc_imu[2, :] measurements = np.vstack((new_mpx, new_mpy, new_mpz, mx, my, mz)) imu_pub = rospy.Publisher('imu_hector', Imu, queue_size=20) pose_pub = rospy.Publisher('pose_hector', Pose, queue_size=20) rospy.init_node('nodeA', anonymous=True) rate = rospy.Rate(100) #100hz i = 0 while (not rospy.is_shutdown()) and i < counter: imu_message = Imu() acc_message = Vector3() coordinates_gnss = Point() position_gnss = Pose() acc_message.x = mx[i] acc_message.y = my[i] acc_message.z = mz[i] imu_message.linear_acceleration = acc_message coordinates_gnss.x = new_mpx[i] coordinates_gnss.y = new_mpy[i] coordinates_gnss.z = new_mpz[i] position_gnss.position = coordinates_gnss imu_pub.publish(imu_message) pose_pub.publish(position_gnss)
def __init__(self, msg): self.id = msg.id self.diagstatus = msg.diagstatus self.gpoint = Point() self.imu = Imu() self.health = KeyValue()
def msg_process(self, msg): msg_list = msg.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] pkgSizeOrient = int(msg_list[moduleSizeAcc + moduleSizeGyro]) moduleSizeOrient = 4 * pkgSizeOrient + 2 orient_msg = msg_list[moduleSizeAcc + moduleSizeGyro + 2:moduleSizeAcc + moduleSizeGyro + moduleSizeOrient] biggerpkg = max([pkgSizeAcc, pkgSizeGyro, pkgSizeOrient]) imu_list = [] acceleration = Vector3() velocity = Vector3() orientation = Quaternion() 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] if i < pkgSizeOrient: orientation = Quaternion() orientation.x = float(orient_msg[3 * i].replace(',', '.')) * 10**-6 orientation.y = float(orient_msg[3 * i + 1].replace( ',', '.')) * 10**-6 orientation.z = float(orient_msg[3 * i + 2].replace( ',', '.')) * 10**-6 orientation.w = 0 time = orient_msg[3 * pkgSizeOrient + 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_message.orientation = orientation imu_list.append(imu_message) return imu_list
def _create_msg(self, data): """ Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down """ msg1 = Imu() msg1_magfield = MagneticField() msg1.header.stamp = rospy.Time.now() msg1.header.frame_id = '/base_link' msg1_magfield.header.stamp = rospy.Time.now() msg1_magfield.header.frame_id = '/base_link' msg1.linear_acceleration.x = -data["IMU1"]["accel_y"] msg1.linear_acceleration.y = -data["IMU1"]["accel_z"] msg1.linear_acceleration.z = data["IMU1"]["accel_x"] msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1.angular_velocity.x = -data["IMU1"]["gyro_y"] msg1.angular_velocity.y = -data["IMU1"]["gyro_z"] msg1.angular_velocity.z = data["IMU1"]["gyro_x"] msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1.orientation.w = 0 msg1.orientation.x = 0 msg1.orientation.y = 0 msg1.orientation.z = 0 msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1_magfield.magnetic_field.x = -data["IMU1"]["mag_y"] msg1_magfield.magnetic_field.y = -data["IMU1"]["mag_z"] msg1_magfield.magnetic_field.z = data["IMU1"]["mag_x"] msg2 = Imu() msg2_magfield = MagneticField() msg2.header.stamp = rospy.Time.now() msg2.header.frame_id = '/base_link' msg2_magfield.header.stamp = rospy.Time.now() msg2_magfield.header.frame_id = '/base_link' msg2.linear_acceleration.x = data["IMU2"]["accel_y"] msg2.linear_acceleration.y = data["IMU2"]["accel_z"] msg2.linear_acceleration.z = data["IMU2"]["accel_x"] msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2.angular_velocity.x = data["IMU2"]["gyro_y"] msg2.angular_velocity.y = data["IMU2"]["gyro_z"] msg2.angular_velocity.z = data["IMU2"]["gyro_x"] msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2.orientation.w = 0 msg2.orientation.x = 0 msg2.orientation.y = 0 msg2.orientation.z = 0 msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2_magfield.magnetic_field.x = data["IMU2"]["mag_y"] msg2_magfield.magnetic_field.y = data["IMU2"]["mag_z"] msg2_magfield.magnetic_field.z = data["IMU2"]["mag_x"] msg3 = Imu() msg3_magfield = MagneticField() msg3.header.stamp = rospy.Time.now() msg3.header.frame_id = '/base_link' msg3_magfield.header.stamp = rospy.Time.now() msg3_magfield.header.frame_id = '/base_link' msg3.linear_acceleration.x = (msg1.linear_acceleration.x + msg2.linear_acceleration.x) / 2. msg3.linear_acceleration.y = (msg1.linear_acceleration.y + msg2.linear_acceleration.y) / 2. msg3.linear_acceleration.z = (msg1.linear_acceleration.z + msg2.linear_acceleration.z) / 2. msg3.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg3.angular_velocity.x = (msg1.angular_velocity.x + msg2.angular_velocity.x) / 2. msg3.angular_velocity.y = (msg1.angular_velocity.y + msg2.angular_velocity.y) / 2. msg3.angular_velocity.z = (msg1.angular_velocity.z + msg2.angular_velocity.z) / 2. msg3.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg3.orientation.w = 0 msg3.orientation.x = 0 msg3.orientation.y = 0 msg3.orientation.z = 0 msg3.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg3_magfield.magnetic_field.x = (msg1_magfield.magnetic_field.x + msg2_magfield.magnetic_field.x) / 2. msg3_magfield.magnetic_field.y = (msg1_magfield.magnetic_field.y + msg2_magfield.magnetic_field.y) / 2. msg3_magfield.magnetic_field.z = (msg1_magfield.magnetic_field.z + msg2_magfield.magnetic_field.z) / 2. return msg1, msg1_magfield, msg2, msg2_magfield, msg3, msg3_magfield
IMU_FRAME = 'imu_link' grad2rad = 3.141592654 / 180.0 if __name__ == '__main__': rospy.init_node('adis_16488') imu_pub = rospy.Publisher(rospy.get_name() + '/imu_data_raw', Imu) adis_info_pub = rospy.Publisher(rospy.get_name() + '/adis_16488_info', ADIS16488) port = rospy.get_param("~port", "/dev/ttyACM0") baud = int(rospy.get_param("~baud", "115200")) ser = serial.Serial(port, baud, timeout=1) rospy.loginfo('Publishing imu data on topics:\n' + rospy.get_name() + '/imu_data_raw\n' + '/adis_16488_info') imuMsg = Imu() ADISInfo = ADIS16488() 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] yaw = 0.0 pitch = 0.0 roll = 0.0 prod_id = 0.0 rospy.sleep(3) while not rospy.is_shutdown(): line = ser.readline() words = string.split(line, ",") #print words
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/mag_camera", MagneticField, queue_size=1) self.roll_pub = rospy.Publisher("camera_roll", Float64, queue_size=1) self.pitch_pub = rospy.Publisher("camera_pitch", Float64, queue_size=1) self.yaw_pub = rospy.Publisher("camera_yaw", Float64, queue_size=1) #self.mag_pub = rospy.Publisher('imu/camera_mag', MagneticField, queue_size = 1) head1 = 0 head2 = 0 head3 = 0 head4 = 0 head5 = 0 head6 = 0 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) line = ser.readline() rpy = line.split(",", 4) magneticsphere = ([ 1.0974375136949293, 0.008593465160122918, 0.0003032211129407881 ], [0.008593465160122857, 1.1120651652148803, 0.8271491584066613], [ 0.00030322111294077105, 0.0926004380061327, 0.5424720769663267 ]) bias = ([-0.021621641870448665], [-0.10661356710756408], [0.4377993330407842]) raw_values = ([float(magn[1])], [float(magn[2])], [float(magn[3])]) shift = ([raw_values[0][0] - bias[0][0] ], [raw_values[1][0] - bias[1][0]], [raw_values[2][0] - bias[2][0]]) magneticfield = np.dot(magneticsphere, shift) #rospy.loginfo(magneticfield) 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 = [ .01, 0.0, 0.0, 0.0, .01, 0.0, 0.0, 0.0, .01 ] 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 ] mag.header.stamp = rospy.Time.now() mag.header.frame_id = '/base_link' # i.e. '/odom' mag.magnetic_field.x = float(magn[1]) / 100.0 mag.magnetic_field.y = float(magn[2]) / 100.0 mag.magnetic_field.z = float(magn[3]) / 100.0 mag.magnetic_field_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 ] #magn[2] = -float(magn[2]) #heading = math.atan2(float(magn[2]),float(magn[1]))*(180/math.pi) #heading = math.atan2(float(magn[2]),float(magn[1])) #if heading < 0: # heading += 360 #rospy.loginfo(head[1]) self.roll_pub.publish(float(rpy[1])) self.pitch_pub.publish(float(rpy[2])) self.pitch_pub.publish(float(rpy[3])) self.imu_pub.publish(imu) self.mag_pub.publish(mag) rate.sleep()
magnetic_pub = rospy.Publisher('mag_output', MagneticField, queue_size = 10) rospy.init_node('imu_node', anonymous = True) serial_port = rospy.get_param('~port', '/dev/pts/2') serial_baud = rospy.get_param('~baudrate', 115200) port = serial.Serial(serial_port, serial_baud) rospy.logdebug("Initialization Complete") rospy.loginfo("Publishing IMU Information") #print'debug' try: while not rospy.is_shutdown(): line = port.readline() if '$VNYMR' in line: imu_msg = Imu() mag_msg = MagneticField() imu_msg.header.stamp = rospy.Time.now() imu_msg.header.frame_id = "imu" mag_msg.header.stamp = rospy.Time.now() mag_msg.header.frame_id = "mag" #print "debug1" try: #print "debug2" #print 'debug3' sensor_data = line.split(",")
def update_physics(delay, quadcopterId, quadcopter_controller, cmd_sub, imu_pub, range_pub): vol_meas = p.getBaseVelocity(quadcopterId) one_over_sample_rate = 1 / delay previous_publish = time.perf_counter() try: while not rospy.is_shutdown(): start = time.perf_counter() # the controllers are evaluated at a slower rate, only once in the # control_subsample times the controller is evaluated pos_meas, quaternion_meas = p.getBasePositionAndOrientation( quadcopterId) previous_vol_meas = np.array(vol_meas) vol_meas = np.array(p.getBaseVelocity(quadcopterId)) #print(vol_meas, type(vol_meas)) acc = (vol_meas - previous_vol_meas) * one_over_sample_rate #print("VOL MEAS", previous_vol_meas, acc) if quadcopter_controller.sample == 0: quadcopter_controller.sample = control_subsample #force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas) actual_values = cmd_sub.get_control() vectorizer = np.zeros([5, 3]) vectorizer[:, 2] = actual_values #print("input forces ", actual_values) force_act1, force_act2, force_act3, force_act4, moment_yaw = vectorizer #print("Well well well\t\t", vectorizer, force_act1, moment_yaw) else: quadcopter_controller.sample -= 1 # apply forces/moments from controls etc: # (do this each time, because forces and moments are reset to zero after a stepSimulation()) #assuming right is -y p.applyExternalForce(quadcopterId, -1, force_act1, [arm_length, -arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act2, [-arm_length, -arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act3, [-arm_length, arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act4, [arm_length, arm_length, 0.], p.LINK_FRAME) # for the yaw-control: p.applyExternalTorque(quadcopterId, -1, moment_yaw, p.LINK_FRAME) # do simulation with pybullet: p.stepSimulation() header = Header() header.frame_id = 'Body' header.stamp = rospy.Time.now() Tsample_physics imu_message = Imu() imu_message.header = header imu_message.orientation.x = quaternion_meas[0] imu_message.orientation.y = quaternion_meas[1] imu_message.orientation.z = quaternion_meas[2] imu_message.orientation.w = quaternion_meas[3] imu_message.linear_acceleration.x = acc[0, 0] imu_message.linear_acceleration.y = acc[0, 1] imu_message.linear_acceleration.z = acc[0, 2] msg = Range() msg.header = header msg.max_range = 200 msg.min_range = 0 msg.range = pos_meas[2] if start - previous_publish > 1 / 100: imu_pub.publish(imu_message) range_pub.publish(msg) previous_publish = start #print("BUBLISHED RANON /simulation/infrared_sensor_node", msg.range) # delay than repeat calc_time = time.perf_counter() - start if (calc_time > delay): #print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay)) pass else: # print("calc_time = ",calc_time) while (time.perf_counter() - start < delay): time.sleep(delay / 10) except KeyboardInterrupt: import sys print("SYS exit in physcis update") sys.exit()
cov_acc = rospy.get_param('~cov_acc') cov_gnss = rospy.get_param('~cov_gnss') cov_odom = rospy.get_param('~cov_odom') # ROS publishers pub_imu = rospy.Publisher('/imu', Imu, queue_size=10) pub_odom = rospy.Publisher('/odom', Odometry, queue_size=10) pub_gnss = rospy.Publisher('/gnss', Odometry, queue_size=10) # Load IMU data mat = sio.loadmat(file_path) base_time = rospy.get_rostime() imu_data = [] for k in range(len(mat['in_data']['IMU'][0][0]['t'][0][0])): msg = Imu() t = mat['in_data']['IMU'][0][0]['t'][0][0][k][0] msg.header.stamp = base_time + rospy.Duration.from_sec(t) msg.header.frame_id = 'base_link' msg.linear_acceleration.x = mat['in_data']['IMU'][0][0]['acc'][0][0][0][k] msg.linear_acceleration.y = mat['in_data']['IMU'][0][0]['acc'][0][0][1][k] msg.linear_acceleration.z = mat['in_data']['IMU'][0][0]['acc'][0][0][2][k] msg.linear_acceleration_covariance[0] = cov_acc msg.linear_acceleration_covariance[4] = cov_acc msg.linear_acceleration_covariance[8] = cov_acc msg.angular_velocity.x = mat['in_data']['IMU'][0][0]['gyro'][0][0][0][k] msg.angular_velocity.y = mat['in_data']['IMU'][0][0]['gyro'][0][0][1][k] msg.angular_velocity.z = mat['in_data']['IMU'][0][0]['gyro'][0][0][2][k] msg.angular_velocity_covariance[0] = cov_gyro
def controller(self): rate = rospy.Rate(10) # 10hz # self.x = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[0] # update self.x everytime # self.y = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[1] # update self.x everytime while not rospy.is_shutdown(): # for simulating self.vx = self.twist.linear.x self.vy = self.twist.linear.y self.vth = self.twist.angular.z self.current_time = rospy.Time.now() # We do not need this part, we are doing our own localization # compute odometry in a typical way given the velocities of the robot self.dt = (self.current_time - self.last_time).to_sec() self.delta_x = (self.vx * cos(self.th) - self.vy * sin(self.th)) * self.dt self.delta_y = (self.vx * sin(self.th) + self.vy * cos(self.th)) * self.dt self.delta_th = self.vth * self.dt # from GPS # self.target_location = [earth_radius, 41.103465, 29.027909] # publish to move_base_simple/goal # from magnetometer # self.yaw = 0 # X, Y = current location # th = current yaw """ self.x += self.delta_x self.y += self.delta_y self.th += self.delta_th """ # since all odometry is 6DOF we'll need a quaternion created from yaw self.odom_quat = tf.transformations.quaternion_from_euler( 0, 0, self.th) # first, we'll publish the transform over tf """ self.odom_broadcaster.sendTransform( (self.x, self.y, 0.), self.odom_quat, self.current_time, "base_link", "odom" ) """ # next, we'll publish the odometry message over ROS self.odom = Odometry() self.odom.header.stamp = self.current_time self.odom.header.frame_id = "odom" # set the position self.odom.pose.pose = Pose(Point(self.x, self.y, 0.), Quaternion(*self.odom_quat)) # Write a tranform formula for calculating linear.x linear.y and angular.z speed # set the velocity self.odom.child_frame_id = "base_link" self.odom.twist.twist = Twist(Vector3(self.vx, self.vy, 0), Vector3(0, 0, self.vth)) # publish the PoseWithCovarianceStamped self.pwcs = PoseWithCovarianceStamped() self.pwcs.header.stamp = self.odom.header.stamp self.pwcs.header.frame_id = "odom" self.pwcs.pose.pose = self.odom.pose.pose self.last_time = self.current_time # publish the NavSatFix self.sensor_data = None self.gps_fix = NavSatFix() self.gps_fix.header.frame_id = "base_link" self.gps_fix.header.stamp = self.odom.header.stamp self.gps_fix.status.status = 0 # GPS FIXED self.gps_fix.status.service = 1 # GPS SERVICE = GPS # Buralar bizden gelecek self.gps_fix.latitude = 41.24600 self.gps_fix.longitude = 29.123123 self.gps_fix.altitude = 0 self.gps_fix.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] self.gps_fix.position_covariance_type = 0 # publish the NavSatFix self.waypoint = WayPoint() # self.waypoint.id.uuid = [1] self.waypoint.position.latitude = 41.24678 self.waypoint.position.longitude = 29.123100 self.waypoint.position.altitude = 0 # self.waypoint.props.key = "key" # self.waypoint.props.value = "1" # publish the NavSatFix self.imuMsg = Imu() self.imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] self.imuMsg.angular_velocity_covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.imuMsg.linear_acceleration_covariance = [ 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.roll = 0 self.pitch = 0 self.yaw = self.th # şimdilik # Acceloremeter self.imuMsg.linear_acceleration.x = 0 self.imuMsg.linear_acceleration.y = 0 self.imuMsg.linear_acceleration.z = 0 # Gyro self.imuMsg.angular_velocity.x = 0 self.imuMsg.angular_velocity.y = 0 self.imuMsg.angular_velocity.z = 0 self.q = tf.transformations.quaternion_from_euler( self.roll, self.pitch, self.yaw) self.imuMsg.orientation.x = self.q[0] #magnetometer self.imuMsg.orientation.y = self.q[1] self.imuMsg.orientation.z = self.q[2] self.imuMsg.orientation.w = self.q[3] self.imuMsg.header.frame_id = "base_link" self.imuMsg.header.stamp = self.odom.header.stamp # Subscriber(s) rospy.Subscriber('/husky_velocity_controller/cmd_vel', Twist, self.callback) rospy.Subscriber('/odometry/gps', Odometry, self.gps_callback) # Publisher(s) self.odom_pub.publish(self.odom) self.odom_combined_pub.publish(self.pwcs) self.gps_pub.publish(self.gps_fix) self.waypoint_pub.publish(self.waypoint) self.imu_pub.publish(self.imuMsg) rate.sleep()
def _create_msg(self, data): """ Messages published with ROS have axis from the robot frame : x forward, y ?, z up or down """ #----IMU 1 calibrate--- msg1 = Imu() msg1_magfield = MagneticField() msg1.header.stamp = rospy.Time.now() msg1.header.frame_id = '/base_link' msg1_magfield.header.stamp = rospy.Time.now() msg1_magfield.header.frame_id = '/base_link' #For calibration and normalisation of linear_acceleration and magnetometer #Calibrating file are multiplied by 1e3, need to multiply raw acceleration and magnetometer by 1e3 for concordance self.IMU1_accX = self.w * (-data["IMU1"]["accel_y"] - self.calibrationFileIMU1['acc_off_x'] * 1e-3) + (1 - self.w) * self.IMU1_accX self.IMU1_accY = self.w * (-data["IMU1"]["accel_z"] - self.calibrationFileIMU1['acc_off_y'] * 1e-3) + (1 - self.w) * self.IMU1_accY self.IMU1_accZ = self.w * (data["IMU1"]["accel_x"] - self.calibrationFileIMU1['acc_off_z'] * 1e-3) + (1 - self.w) * self.IMU1_accZ msg1.linear_acceleration.x = self.IMU1_accX msg1.linear_acceleration.y = self.IMU1_accY msg1.linear_acceleration.z = self.IMU1_accZ msg1.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1.angular_velocity.x = -data["IMU1"]["gyro_y"] msg1.angular_velocity.y = -data["IMU1"]["gyro_z"] msg1.angular_velocity.z = data["IMU1"]["gyro_x"] msg1.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1.orientation.w = 0 msg1.orientation.x = 0 msg1.orientation.y = 0 msg1.orientation.z = 0 msg1.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1_magfield.magnetic_field.x = ( -data["IMU1"]["mag_y"] * 1e3 - self.calibrationFileIMU1['magn_off_x'] ) / self.calibrationFileIMU1['magn_scale_x'] msg1_magfield.magnetic_field.y = ( -data["IMU1"]["mag_z"] * 1e3 - self.calibrationFileIMU1['magn_off_y'] ) / self.calibrationFileIMU1['magn_scale_y'] msg1_magfield.magnetic_field.z = ( data["IMU1"]["mag_x"] * 1e3 - self.calibrationFileIMU1['magn_off_z'] ) / self.calibrationFileIMU1['magn_scale_z'] #----IMU 1 raw -------- msg1_raw = Imu() msg1_magfield_raw = MagneticField() msg1_raw.header.stamp = rospy.Time.now() msg1_raw.header.frame_id = '/base_link' msg1_magfield_raw.header.stamp = rospy.Time.now() msg1_magfield_raw.header.frame_id = '/base_link' msg1_raw.linear_acceleration.x = -data["IMU1"]["accel_y"] msg1_raw.linear_acceleration.y = -data["IMU1"]["accel_z"] msg1_raw.linear_acceleration.z = data["IMU1"]["accel_x"] msg1_raw.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1_raw.angular_velocity.x = -data["IMU1"]["gyro_y"] msg1_raw.angular_velocity.y = -data["IMU1"]["gyro_z"] msg1_raw.angular_velocity.z = data["IMU1"]["gyro_x"] msg1_raw.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1_raw.orientation.w = 0 msg1_raw.orientation.x = 0 msg1_raw.orientation.y = 0 msg1_raw.orientation.z = 0 msg1_raw.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg1_magfield_raw.magnetic_field.x = -data["IMU1"]["mag_y"] msg1_magfield_raw.magnetic_field.y = -data["IMU1"]["mag_z"] msg1_magfield_raw.magnetic_field.z = data["IMU1"]["mag_x"] #----IMU 2 calibrate--- msg2 = Imu() msg2_magfield = MagneticField() msg2.header.stamp = rospy.Time.now() msg2.header.frame_id = '/base_link' msg2_magfield.header.stamp = rospy.Time.now() msg2_magfield.header.frame_id = '/base_link' self.IMU2_accX = self.w * (data["IMU2"]["accel_y"] - self.calibrationFileIMU2['acc_off_x'] * 1e-3) + (1 - self.w) * self.IMU2_accX self.IMU2_accY = self.w * (data["IMU2"]["accel_z"] - self.calibrationFileIMU2['acc_off_y'] * 1e-3) + (1 - self.w) * self.IMU2_accY self.IMU2_accZ = self.w * (data["IMU2"]["accel_x"] - self.calibrationFileIMU2['acc_off_z'] * 1e-3) + (1 - self.w) * self.IMU2_accZ msg2.linear_acceleration.x = self.IMU2_accX msg2.linear_acceleration.y = self.IMU2_accY msg2.linear_acceleration.z = self.IMU2_accZ msg2.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2.angular_velocity.x = data["IMU2"]["gyro_y"] msg2.angular_velocity.y = data["IMU2"]["gyro_z"] msg2.angular_velocity.z = data["IMU2"]["gyro_x"] msg2.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2.orientation.w = 0 msg2.orientation.x = 0 msg2.orientation.y = 0 msg2.orientation.z = 0 msg2.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2_magfield.magnetic_field.x = ( data["IMU2"]["mag_y"] * 1e3 - self.calibrationFileIMU2['magn_off_x'] ) / self.calibrationFileIMU2['magn_scale_x'] msg2_magfield.magnetic_field.y = ( data["IMU2"]["mag_z"] * 1e3 - self.calibrationFileIMU2['magn_off_y'] ) / self.calibrationFileIMU2['magn_scale_y'] msg2_magfield.magnetic_field.z = ( data["IMU2"]["mag_x"] * 1e3 - self.calibrationFileIMU2['magn_off_z'] ) / self.calibrationFileIMU2['magn_scale_z'] #----IMU 2 raw -------- msg2_raw = Imu() msg2_magfield_raw = MagneticField() msg2_raw.header.stamp = rospy.Time.now() msg2_raw.header.frame_id = '/base_link' msg2_magfield_raw.header.stamp = rospy.Time.now() msg2_magfield_raw.header.frame_id = '/base_link' msg2_raw.linear_acceleration.x = data["IMU2"]["accel_y"] msg2_raw.linear_acceleration.y = data["IMU2"]["accel_z"] msg2_raw.linear_acceleration.z = data["IMU2"]["accel_x"] msg2_raw.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2_raw.angular_velocity.x = data["IMU2"]["gyro_y"] msg2_raw.angular_velocity.y = data["IMU2"]["gyro_z"] msg2_raw.angular_velocity.z = data["IMU2"]["gyro_x"] msg2_raw.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2_raw.orientation.w = 0 msg2_raw.orientation.x = 0 msg2_raw.orientation.y = 0 msg2_raw.orientation.z = 0 msg2_raw.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg2_magfield_raw.magnetic_field.x = data["IMU2"]["mag_y"] msg2_magfield_raw.magnetic_field.y = data["IMU2"]["mag_z"] msg2_magfield_raw.magnetic_field.z = data["IMU2"]["mag_x"] return msg1, msg1_magfield, msg1_raw, msg1_magfield_raw, msg2, msg2_magfield, msg2_raw, msg2_magfield_raw
def imu_callback(imu_raw): global imu, mag, is_calibrated, samples now = imu_raw.header.stamp ############# Po mereni smazat ##################### raw_vector6 = Imu() raw_vector6.header.stamp = now raw_vector6.linear_acceleration.x = imu_raw.raw_linear_acceleration.x raw_vector6.linear_acceleration.y = imu_raw.raw_linear_acceleration.y raw_vector6.linear_acceleration.z = imu_raw.raw_linear_acceleration.z raw_vector6.angular_velocity.x = imu_raw.raw_angular_velocity.x raw_vector6.angular_velocity.y = imu_raw.raw_angular_velocity.y raw_vector6.angular_velocity.z = imu_raw.raw_angular_velocity.z imu_raw_publisher.publish(raw_vector6) ####################################################### if not (is_calibrated): acc_bias[0] = acc_bias[0] + imu_raw.raw_linear_acceleration.x acc_bias[1] = acc_bias[1] + imu_raw.raw_linear_acceleration.y acc_bias[2] = acc_bias[2] + imu_raw.raw_linear_acceleration.z gyro_bias[0] = gyro_bias[0] + imu_raw.raw_angular_velocity.x gyro_bias[1] = gyro_bias[1] + imu_raw.raw_angular_velocity.y gyro_bias[2] = gyro_bias[2] + imu_raw.raw_angular_velocity.z samples += 1 if samples >= SamplesNumber: is_calibrated = True acc_bias[0] = acc_bias[0] / SamplesNumber acc_bias[1] = acc_bias[1] / SamplesNumber acc_bias[2] = acc_bias[2] / SamplesNumber gyro_bias[0] = gyro_bias[0] / SamplesNumber gyro_bias[1] = gyro_bias[1] / SamplesNumber gyro_bias[2] = gyro_bias[2] / SamplesNumber rospy.loginfo('IMU calibrated') return ## Correct imu data with calibration value imu.header.stamp = now # Accelerometer imu.linear_acceleration.x = imu_raw.raw_linear_acceleration.x - acc_bias[0] imu.linear_acceleration.y = imu_raw.raw_linear_acceleration.y - acc_bias[1] imu.linear_acceleration.z = imu_raw.raw_linear_acceleration.z - acc_bias[ 2] + 9.81 # Gyroscope imu.angular_velocity.x = imu_raw.raw_angular_velocity.x - gyro_bias[0] imu.angular_velocity.y = imu_raw.raw_angular_velocity.y - gyro_bias[1] imu.angular_velocity.z = imu_raw.raw_angular_velocity.z - gyro_bias[2] # Magnetic mag_raw = array([ imu_raw.raw_magnetic_field.x, imu_raw.raw_magnetic_field.y, imu_raw.raw_magnetic_field.z ]) mag_raw = multiply(mag_raw, 1e-6) # Convert from microTesla to Tesla mag_cal = M_matrix.dot((mag_raw - M_bias.T).T).T mag.magnetic_field.x = mag_cal[0] mag.magnetic_field.y = mag_cal[1] mag.magnetic_field.z = mag_cal[2] mag.header.stamp = now ## Publish topics imu_publisher.publish(imu) imu_mag_publisher.publish(mag)
buf_out.append(data) try: ser.write(buf_out) buf_in = bytearray(ser.read(2)) # print("Writing, wr: ", binascii.hexlify(buf_out), " re: ", binascii.hexlify(buf_in)) except: return False if (buf_in.__len__() != 2) or (buf_in[1] != 0x01): #rospy.logerr("Incorrect Bosh IMU device response.") return False return True imu_data = Imu() # Filtered data imu_raw = Imu() # Raw IMU data temperature_msg = Temperature() # Temperature mag_msg = MagneticField() # Magnetometer data # Main function if __name__ == '__main__': rospy.init_node("bosch_imu_node") # Sensor measurements publishers pub_data = rospy.Publisher('imu/data', Imu, queue_size=1) pub_raw = rospy.Publisher('imu/raw', Imu, queue_size=1) pub_mag = rospy.Publisher('imu/mag', MagneticField, queue_size=1) pub_temp = rospy.Publisher('imu/temp', Temperature, queue_size=1) # srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
#!/usr/bin/env python import sys import rospy import numpy from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3Stamped, Vector3 import tf.transformations as trans import time imu_data = Imu() gps_vel = Vector3Stamped() def imu_cb(msg): global imu_data imu_data = msg def vel_cb(msg): global gps_vel gps_vel = msg def vel_fix(vel): q = imu_data.orientation q = numpy.array([q.x, q.y, q.z, q.w]) org_vel = numpy.array([vel.x, vel.y, vel.z]) trans_vel = trans.quaternion_matrix(q).transpose()[0:3, 0:3].dot(org_vel) corr_vel = Vector3(trans_vel[0], trans_vel[1], trans_vel[2]) # print ("corrected vel: \n{}".format(corr_vel))
# Date :22/11/2106 # This code merges stereo odometry with IMU and command velocities to produce an estimate of the HUSKY's pose and heading import rospy import tf.transformations from geometry_msgs.msg import Twist from geometry_msgs.msg import TwistStamped from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry import math import numpy as np pub = rospy.Publisher("/odometry/filtered/self", Odometry, queue_size=10) br = tf.TransformBroadcaster() odom = Odometry() imu = Imu() yawViso = 0.0 vYawViso = 0.0 vX = 0.0 yawImu = 0.0 yawImu1 = 0.0 yaw = 0.0 i = 0 vYawCov = 0.09 ImuYawCov = 0.15707963267948966 yawCov = 10 cmd = np.array([(0.0, 0.0), (0.0, 0.0)]) yaw = np.array([0.0, 0.0]) meas = np.array([(0.0, 0.0), (0.0, 0.0)]) # meas = np.array ([0.0,0.0]) inp = np.array([0.0, 0.0])
def run(self): while not rospy.is_shutdown(): self.lock.acquire() self.current_time = time.time() self.last_tiem = self.current_time # self.zumy.cmd(0.1,0.1) # print imu and enc data # self.zumy.read_data() imu_data = self.zumy.read_imu() enc_data = self.zumy.read_enc() # imu_data = [0,0,0,0,0,0] # enc_data = [0,0] self.lock.release() twistImu = Twist() kalman_msg = kalman() if len(imu_data) == 6: imu_msg = Imu() imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name) imu_msg.linear_acceleration.x = 9.81 * imu_data[0] imu_msg.linear_acceleration.y = 9.81 * imu_data[1] imu_msg.linear_acceleration.z = 9.81 * imu_data[2] imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3] imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4] imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5] #twistImu.angular.x = 0 #twistImu.angular.y = 0 #twistImu.angular.z = imu_msg.angular_velocity.z self.imu_pub.publish(imu_msg) # kalman_msg = kalman() kalman_msg.keyboard_linear = self.v kalman_msg.keyboard_angular = -self.a*3.14 vvv = self.v aaa = self.a*3.14 self.angular_pub.publish(-aaa) self.linear_pub.publish(vvv) kalman_msg.measure_angular = imu_msg.angular_velocity.z # self.kalman_pub.publish(kalman_msg) self.nonekalman_angular.publish(kalman_msg.measure_angular) if len(enc_data) == 2: enc_msg = Int16() enc_dif_r = Int16() enc_dif_l = Int16() #global enc_data_r #global enc_data_l self.lock.acquire() ly_daenc = numpy.load('ly_enc.npy') enc_msg.data = enc_data[0] self.lock.release() self.r_enc_pub.publish(enc_msg) #ly_daenc[0] = enc_data[0] self.lock.acquire() enc_data_r = ly_daenc[0] enc_dif_r.data = enc_msg.data - enc_data_r # difference value enc_msg.data = enc_data[1] self.lock.release() self.l_enc_pub.publish(enc_msg) #ly_daenc[1] = enc_data[1] self.lock.acquire() enc_data_l = ly_daenc[1] enc_dif_l.data = enc_msg.data - enc_data_l # difference value ly_daenc = enc_data numpy.save('ly_enc.npy',ly_daenc) #this get the robocar`s wheels of velocity # velocity =(( encoder`s value of change in one cycle ) / total number of encoder) * wheel`s perimeter enc_sr = (float(enc_dif_r.data)/120.0)*30*3.14 # enc_vr =float(enc_dif_r /1167)*17.25 # it don`t work!!!wtf enc_vr = enc_sr * 0.01725 enc_sl = (float(enc_dif_l.data)/120.0)*30*3.14 # enc_vl =float(enc_dif_l /1167)*17.25 enc_vl = enc_sl * 0.01725 #print enc_dif_r,'dif_r' #print enc_dif_l,'dif_l' #print enc_sr,'sr' #print enc_sl,'sl' #print enc_vr,'vr' #print enc_vl,'vl' self.lock.release() enc_v_v = self.enc_vel(enc_vr,enc_vl) enc_v = enc_v_v/4.4 if self.v > self.hey or self.v < self.hey: if enc_v == self.hey: enc_v = self.enc_v_last self.enc_v_last = enc_v print self.enc_v_last ,'enc_v' enc_r_r = self.enc_rad(enc_vr,enc_vl) enc_r = enc_r_r/3 if self.a > self.hey or self.a < self.hey: if enc_r == 0: enc_r = self.enc_r_last self.enc_r_last = enc_r print self.enc_r_last ,'enc_r' twistImu.linear.x = self.enc_v_last twistImu.linear.y = 0 twistImu.linear.z = 0 twistImu.angular.x = 0 twistImu.angular.y = 0 twistImu.angular.z = self.enc_r_last kalman_msg.linear_vel = self.enc_v_last self.kalman_pub.publish(kalman_msg) self.enc_angular.publish(self.enc_r_last) self.nonekalman_linear.publish(self.enc_v_last) self.current_time = time.time() # time_dif = self.current_time - self.last_time # self.last_time = self.current_time # print time_dif,'tiem_dif' # numpy.save('ly_enc.npy',ly_daenc) #self.ly_twist_pub.publish(twistImu) #enc_data_r = enc_data[0]#keep last value # enc_data_l = enc_data[1] # print enc_data_r # print enc_data_l # self.lock.acquire() # linear_output = self.PID.update(self.enc_v_last) # angular_output = self.PID_another.update(enc_r) # print output,'output' # print linear_output,'linear_output' # v = linear_output/2 # r = v + (0.2*self.a) # l =(v/1.15) - (0.2*self.a) # print a,'a' # self.lock.acquire() # vv = v*2 # self.sudu_pub.publish(vv) # self.lock.acquire() # self.zumy.cmd(l,r) # self.lock.release() time_dif = self.current_time - self.last_time self.last_time = self.current_time # print time_dif,'tiem_dif' # self.sudu_pub.publish(vv) #self.ly_twist_pub.publish(twistImu) self.rate.sleep()