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()