215 lines
5.6 KiB
Python
215 lines
5.6 KiB
Python
#!/usr/bin/python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
import time
|
|
import can
|
|
import cantools
|
|
import rospy
|
|
import signal
|
|
import os
|
|
import threading
|
|
import std_msgs
|
|
|
|
is_exit = False
|
|
|
|
messages = [
|
|
{
|
|
'message_id': 0x1820FF2F, #messages1
|
|
'message_name': 'Handshaking',
|
|
'signals': {
|
|
'HandShake_name1': 0x74,
|
|
'HandShake_name2': 0x63,
|
|
'HandShake_name3': 0x64,
|
|
'HandShake_name4': 0x72,
|
|
'HandShake_name5': 0x69,
|
|
'HandShake_name6': 0x30,
|
|
'HandShake_name7': 0x30,
|
|
'HandShake_name8': 0x31
|
|
}
|
|
},
|
|
{
|
|
'message_id': 0x0C20112F, # messages4
|
|
'message_name': 'ElectronicHandbrake_State',
|
|
'signals': {
|
|
'EleHandbrakeStatus': 0,
|
|
'Ele_activate_Status': 0,
|
|
'Ele_checksum': 0,
|
|
'Ele_count': 1,
|
|
'reserve14': 0,
|
|
'reserve15': 0,
|
|
'reserve16': 0,
|
|
'reserve17': 0,
|
|
'reserve18': 0
|
|
}
|
|
},
|
|
{
|
|
'message_id': 0x0C20302F, # messages7
|
|
'message_name': 'GearPosition',
|
|
'signals': {
|
|
'GearCommand': 0,
|
|
'GearHighest': 6,
|
|
'gear_activate': 0,
|
|
'gear_checksum': 0,
|
|
'gear_count': 1,
|
|
'reserve19': 0,
|
|
'reserve20': 0,
|
|
'reserve21': 0,
|
|
'reserve22': 0
|
|
}
|
|
},
|
|
{
|
|
'message_id': 0x0C20102F, #messages3
|
|
'message_name': 'Brake_decel',
|
|
'signals': {
|
|
'BrakeDecel': 0,
|
|
'Decel_activate_Status': 0,
|
|
'Decel_checksum': 0,
|
|
'Decel_count': 1,
|
|
'reserve5':225,
|
|
'reserve6':225,
|
|
'reserve7':225,
|
|
'reserve8':225
|
|
}
|
|
},#41 74 FF FF FF FF 00 3F
|
|
{
|
|
'message_id': 0x0C20122F, #messages5
|
|
'message_name': 'RetarderTorque_Percent',
|
|
'signals': {
|
|
'Retarder_per': 0,
|
|
'Retarder_activate': 0,
|
|
'Retarder_checksum': 0,
|
|
'Retarder_count': 1,
|
|
'reserve23':0,
|
|
'reserve24':0,
|
|
'reserve25':0,
|
|
'reserve26':0,
|
|
'reserve27':0
|
|
|
|
}
|
|
},
|
|
{
|
|
'message_id': 0x0C20002F, # messages2
|
|
'message_name': 'AccPedal_percent',
|
|
'signals': {
|
|
'AccPedal_per': 0,
|
|
'Acc_activate_Status': 0,
|
|
'Acc_activate': 0,
|
|
'Acc_checksum': 0,
|
|
'Acc_count': 1,
|
|
'reserve1': 0,
|
|
'reserve2': 0,
|
|
'reserve3': 0,
|
|
'reserve4': 0
|
|
}
|
|
},
|
|
{
|
|
'message_id': 0x0C20212F, #messages6
|
|
'message_name': 'SteerAngle_Control',
|
|
'signals': {
|
|
'Curvature_command': 0,
|
|
'steer_control': 0,
|
|
'steer_activate': 0,
|
|
'steer_checksum': 0,
|
|
'steer_count': 1,
|
|
'steercontrol_model': 1,
|
|
'reserve28': 0
|
|
|
|
}
|
|
},
|
|
{
|
|
'message_id': 0x0C20402F, #messages8
|
|
'message_name': 'Bucket_State',
|
|
'signals': {
|
|
'bucket_command': 0,
|
|
'bucket_activate': 0,
|
|
'bucket_checksum': 0,
|
|
'bucket_count': 1,
|
|
'bucketspeed_model':0,
|
|
'reserve11':0,
|
|
'reserve12':0,
|
|
'reserve13':0,
|
|
'reserve9':0
|
|
}
|
|
}
|
|
]
|
|
#加载dbc文件
|
|
db_file = os.path.join(os.path.abspath('.'), 'src/wirecontrol/config/kk_wiredcontrol.dbc')
|
|
|
|
database = cantools.database.load_file(db_file)
|
|
|
|
can_interface = 'socketcan' #根据can接口类型进行更改
|
|
bus = can.interface.Bus(channel= 'can0', bustype = can_interface, bitrate = 500000)
|
|
|
|
cnt = 0
|
|
hands_flag = False
|
|
|
|
def signal_handler(signal, frame):
|
|
global is_exit
|
|
is_exit = True
|
|
|
|
def callback_from_rviz(msg):
|
|
global hands_flag
|
|
print("hand brake!!!!!")
|
|
hands_flag = False
|
|
|
|
def send_can_messages(messages):
|
|
global cnt
|
|
if cnt >= 15:
|
|
cnt = 1
|
|
else:
|
|
cnt += 1
|
|
# 发送多个CAN消息
|
|
for message in messages:
|
|
# 创建CAN消息对象
|
|
print(message)
|
|
msg = database.get_message_by_name(message['message_name'])
|
|
data = msg.encode(message['signals'])
|
|
data1 = list(data)
|
|
if message['message_id'] != 0x1820FF2F:
|
|
len0 = len(data1) - 1
|
|
i = 0
|
|
sum = 0
|
|
while i < len0:
|
|
sum = sum + data1[i]
|
|
i = i + 1
|
|
sum = sum + cnt
|
|
data1[len0] = (cnt << 4) | ((sum >> 4) + sum) & 0x0f
|
|
else:
|
|
pass
|
|
can_message = can.Message(arbitration_id=msg.frame_id, data=data1, is_extended_id=True)
|
|
|
|
# 发送CAN消息
|
|
bus.send(can_message)
|
|
|
|
def shakehand_Task():
|
|
global hands_flag
|
|
global messages
|
|
|
|
cnt1 = 0
|
|
while cnt1 <= 0.5:
|
|
cnt1 += 0.01
|
|
send_can_messages(messages)
|
|
time.sleep(0.01)
|
|
print("shakehand task finish!")
|
|
|
|
def main():
|
|
global is_exit
|
|
global hands_flag
|
|
|
|
signal.signal(signal.SIGINT, signal_handler)
|
|
rospy.init_node('wirecontrol_sh', anonymous=False)
|
|
rospy.Subscriber("/Handshack", std_msgs.msg.String, callback_from_rviz)
|
|
|
|
while not is_exit:
|
|
if hands_flag == False:
|
|
hands_flag = True
|
|
ShakehandTask_threading = threading.Thread(target = shakehand_Task)
|
|
ShakehandTask_threading.start()
|
|
|
|
time.sleep(1)
|
|
rospy.spin()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|