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