def __init__(self): print "Initializing" self._Counter = 0 self._ax = 0 port = rospy.get_param("~port", "/dev/gyro") baudRate = int(rospy.get_param("~baudRate", 115200)) rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate)) self._BananaSerial = BananaSerial(port, baudRate, self._HandleReceivedLine) rospy.loginfo("Started serial communication") #Publisher for entire serial data self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10) #Subscribers and Publishers of IMU data topic self.frame_id = '/IMU_link' self.cal_buffer =[] self.cal_buffer_length = 1000 self.imu_data = Imu(header=rospy.Header(frame_id="IMU_link")) self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6] self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0] self.yaw_pub = rospy.Publisher('yaw_from_imu', Float64, queue_size = 10) self.imu_pub = rospy.Publisher('imu_data', Imu, queue_size = 10)
class GYRO_Class(object): def __init__(self): print "Initializing" self._Counter = 0 self._ax = 0 port = rospy.get_param("~port", "/dev/ttyUSB1") baudRate = int(rospy.get_param("~baudRate", 115200)) rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate)) self._BananaSerial = BananaSerial(port, baudRate, self._HandleReceivedLine) rospy.loginfo("Started serial communication") #Publisher for entire serial data self._SerialPublisher = rospy.Publisher('serial', String, queue_size=10) #Subscribers and Publishers of IMU data topic self.frame_id = 'angelbot_mpu6050_link' self.cal_buffer = [] self.cal_buffer_length = 1000 self.imu_data = Imu(header=rospy.Header( frame_id="angelbot_mpu6050_link")) self.imu_data.orientation_covariance = [ 1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6 ] self.imu_data.angular_velocity_covariance = [ 1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6 ] self.imu_data.linear_acceleration_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ] self.yaw_pub = rospy.Publisher('yaw_from_imu', Float64, queue_size=10) self.imu_pub = rospy.Publisher('imu_data', Imu, queue_size=10) 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] == 'yaw'): self._ax = float(lineParts[1]) yaw_msg = Float64() imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h yaw_msg.data = self._ax q = quaternion_from_euler(0, 0, self._ax * -degrees2rad) imu_msg.orientation.x = q[0] imu_msg.orientation.y = q[1] imu_msg.orientation.z = q[2] imu_msg.orientation.w = q[3] imu_msg.orientation_covariance = [ 1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6 ] imu_msg.angular_velocity_covariance = [ 1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6 ] imu_msg.linear_acceleration_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ] self.imu_pub.publish(imu_msg) self.yaw_pub.publish(yaw_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass def Start(self): rospy.logdebug("Starting") self._BananaSerial.Start() def Stop(self): rospy.logdebug("Stopping") self._BananaSerial.Stop()
class GYRO_Class(object): def __init__(self): print "Initializing" self._Counter = 0 self._qx = 0 self._qy = 0 self._qz = 0 self._qw = 0 self._lx = 0 self._ly = 0 self._lz = 0 self._ax = 0 self._ay = 0 self._az = 0 port = rospy.get_param("~port", "/dev/ttyUSB0") baudRate = int(rospy.get_param("~baudRate", 115200)) rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate)) self._BananaSerial = BananaSerial(port, baudRate, self._HandleReceivedLine) rospy.loginfo("Started serial communication") #Publisher for entire serial data self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10) #Subscribers and Publishers of IMU data topic self.frame_id = '/IMU_link' self.cal_buffer =[] self.cal_buffer_length = 1000 self.imu_data = Imu(header=rospy.Header(frame_id="IMU_link")) self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0] self.imu_pub = rospy.Publisher('imu_data', Imu,queue_size = 10) 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 Start(self): rospy.logdebug("Starting") self._BananaSerial.Start() def Stop(self): rospy.logdebug("Stopping") self._BananaSerial.Stop()