def message_received(msg):
global can_pub
global can_ids
global can_pos
global can_pow
global can_last
global can_better_map
rospy.loginfo('message received')
# Seperate final_thrust_msg
del can_pow[:]
can_pow.append(msg.hfl)
can_pow.append(msg.hfr)
can_pow.append(msg.hbr)
can_pow.append(msg.hbl)
can_pow.append(msg.vfl)
can_pow.append(msg.vfr)
can_pow.append(msg.vbr)
can_pow.append(msg.vbl)
base_board = min(can_ids)
max_board = max(can_ids)
for cid in range(base_board, max_board + 1):
data_list = 0
cur = can_better_map[cid]
for el in cur:
if el is not None:
data_list += can_pow[el]
else:
data_list += 127
data_list = data_list << 8
data_list = data_list >> 8
"""
print("|{}|".format(cid))
for i in range(8):
print("{}".format((data_list >> 8*i) & 0xff))
print("----")
"""
# Publish Message
new_msg = can_msg()
new_msg.id = cid
new_msg.data = data_list
if checkChange(new_msg):
can_pub.publish(new_msg)
# TODO Translate message to esc_single_msg
# sample_message = esc_single_msg()
# esc_pub.publish(sample_message)
pass
def bus_message_received(can_rx):
global can_bus
global pub
data_list = list(can_rx.data)
data = 0l
shift = len(data_list) * 8
for i in data_list:
shift -= 8
data = data + (i << shift)
rospy.loginfo('Can Message Received: ' + str(can_rx.arbitration_id) + ':' + str(list(can_rx.data)))
can_rx = can_msg(can_rx.arbitration_id, data)
pub.publish(can_rx)
import rospy from shared_msgs.msg import can_msg, tools_command_msg TOOLS_BOARD_ID = 0x204 MANIPULATOR_OPEN_BIT = 0x04 MANIPULATOR_CLOSE_BIT = 0x40 MARKER_OPEN_BIT = 0x80 MARKER_CLOSE_BIT = 0x08 GROUT_TROUT_OPEN_BIT = 0x10 GROUT_TROUT_CLOSE_BIT = 0x01 LIFT_BAG_OPEN_BIT = 0x02 LIFT_BAG_CLOSE_BIT = 0x20 cmsg = can_msg() cmsg_pm = can_msg() cmsg_gt = can_msg() cmsg_lb = can_msg() cmsg_mk = can_msg() cmsg_pm.id = TOOLS_BOARD_ID cmsg_gt.id = TOOLS_BOARD_ID cmsg_lb.id = TOOLS_BOARD_ID cmsg_mk.id = TOOLS_BOARD_ID changed = False pseudo_lock = False # Scott switched hoses, since only PM wasn't working """