def mb1swing(msg):
    if mode !=2:
        send = CAN()
        send.stdId = 0x122
        send.extId = -1
        send.data = struct.pack('f', msg.data)
        pub.publish( send )
def autoSwingCallback(msg):
    if mode == 2:
        send = CAN()
        send.stdId = 0x122
        send.extId = -1
        send.data = struct.pack('f', msg.data)
        pub.publish(send)
def autoSwingCallback(msg):
    if mode ==2:
        send = CAN()
        send.stdId = 0x122
        send.extId = -1
        send.data = struct.pack('f', msg.data)
        pub.publish( send )
def mb1swing(msg):
    if mode != 2:
        send = CAN()
        send.stdId = 0x122
        send.extId = -1
        send.data = struct.pack('f', msg.data)
        pub.publish(send)
def handCallback(msg):
    send = CAN()
    send.stdId = 0x150
    send.extId = -1
    if msg.data:
        send.data = [0xff]
    else:
        send.data = [0x00]
    pub.publish( send )
def mb1swing(msg):
    if mode !=2:
        send = CAN()
    	send.stdId = 0x150
    	send.extId = -1
    	if msg.data:
        	send.data = [0xff]
    	else:
        	send.data = [0x00]
        pub.publish( send )
Exemple #7
0
def talker():
    pub = rospy.Publisher('cantx', CAN, queue_size=100)
    rospy.init_node('canusb_test', anonymous=True)
    r = rospy.Rate(10)  # 10Hz
    count = 0
    while not rospy.is_shutdown():
        msg = CAN()
        msg.stdId = 1
        msg.extId = -1
        msg.data = struct.pack('B', count % 256) + 'ABCDE'
        pub.publish(msg)

        msg = CAN()
        msg.stdId = 100
        msg.extId = 25
        msg.data = struct.pack('i', count)
        pub.publish(msg)

        count += 1

        r.sleep()
 def analyze(self, str):
     command = str[:1]
     if command == 't':
         self.msg = CAN()
         self.dlc = int(str[4:5],16)
         self.msg.timestamp = rospy.get_rostime()
         self.msg.stdId = int(str[1:4],16)
         self.msg.extId = -1
         for i in range(0, self.dlc):
             self.msg.data +=  struct.pack('B', int(str[5+i*2:7+i*2],16))
         return self.msg
     elif command == 'T':
         self.msg = CAN()
         self.dlc = int(str[9:10],16)
         self.msg.timestamp = rospy.get_rostime()
         self.msg.stdId = int(str[1:9],16)%0x400
         self.msg.extId = int(str[1:9],16)/0x400
         for i in range(0, self.dlc):
             self.msg.data +=  struct.pack('B', int(str[10+i*2:12+i*2],16))
         return self.msg
     else:
         return 0
def handCallback(msg):
    send = CAN()
    send.stdId = 0x150
    send.extId = -1
    if msg.data:
        send.data = [0xff]
    else:
        send.data = [0x00]
    pub.publish(send)
def autoSwingCallback(msg):
    if mode == 2:
        send = CAN()
        send.stdId = 0x150
        send.extId = -1
        if msg.data:
            if isright == True:
                send.data = [0xfb]
            else:
                send.data = [0xfe]
        else:
            send.data = [0x00]
        pub.publish(send)
def mb1swing(msg):
    if mode != 2:
        send = CAN()
        send.stdId = 0x150
        send.extId = -1
        if msg.data:
            send.data = [0xff]
        else:
            send.data = [0x00]
        pub.publish(send)
def autoSwingCallback(msg):
    if mode ==2:
        send = CAN()
    	send.stdId = 0x150
    	send.extId = -1
    	if msg.data:
            if isright == True:
                send.data = [0xfb]
            else:
                send.data = [0xfe]
    	else:
        	send.data = [0x00]
        pub.publish( send )
def mb1motor2(msg):
    send = CAN()
    send.stdId = 0x121
    send.extId = -1
    send.data = struct.pack('f', msg.data)
    pub.publish(send)
def omni3(msg):
    send = CAN()
    send.stdId = 0x112
    send.extId = -1
    send.data = struct.pack('f', msg.data)
    pub.publish(send)
def mb1motor2(msg):
    send = CAN()
    send.stdId = 0x121
    send.extId = -1
    send.data = struct.pack('f', msg.data)
    pub.publish( send )
def omni3(msg):
    send = CAN()
    send.stdId = 0x112
    send.extId = -1
    send.data = struct.pack('f', msg.data)
    pub.publish( send )