def send_datagram(self, sender, receiver, data):
with self.lock:
datagram = xm_DataGram()
datagram.data = data
datagram.sender = sender
datagram.receiver = receiver
self.serial_pub.publish(datagram)
odom_data = [0xf1]
fire_data = [0x11]
wheel_radius = 0.075
# used to calculate the odometry
now_stamp_odom = 0.0
past_stamp_odom = 0.0
now_odom = Odometry()
past_odom = Odometry()
# used to calculate the yaw and pitch
yaw = pitch = 0
yaw_p = pitch_p = 0
now_stamp_gim = 0.0
past_stamp_gim = 0.0
# check the callback execute or not
updated = 0
raw_data = xm_DataGram()
class Can_Node:
def calc_pub_inverse_odom(self, data):
print "odom arrived"
#cycle num and line num ,distance = (cyc * encoder_line + line) / encoder_line * wheel_radius * math.pi
global now_stamp_odom, past_stamp_odom, now_odom, past_odom, updated
w1_cyc, w1_line, w2_cyc, w2_line, w3_cyc, w3_line, w4_cyc, w4_line, = struct.unpack('!hhhhhhhh', data.data)
now_stamp_odom = rospy.get_time()
dT = now_stamp_odom - past_stamp_odom
if dT > (1.5 / self.odom_rate):
print "Odom data delay a lot, please check the rate or serial port"
w1_cyc_p = w1_cyc
w2_cyc_p = w2_cyc
w3_cyc_p = w3_cyc
