def createDatabase(name,filename):
db = kvadblib.Dbc(name=name)
for _msg in _messages:
message = db.new_message(name=_msg.name,id=_msg.id,dlc=_msg.dlc,)
for _sig in _msg.signals:
if isinstance(_sig,EnumSignal):
_type = kvadblib.SignalType.ENUM_UNSIGNED
_enums= _sig.enums
else:
_type = kvadblib.SignalType.UNSIGNED
_enums= {}
message.new_signal(
name = _sig.name,
type = _type,
byte_order = kvadblib.SignalByteOrder.INTEL,
mode = kvadblib.SignalMultiplexMode.MUX_INDEPENDENT,
size = kvadblib.ValueSize(*_sig.size), # * 将元组解包
scaling = kvadblib.ValueScaling(*_sig.scaling),
limits = kvadblib.ValueLimits(*_sig.limits),
unit = _sig.unit,
enums = _enums,
)
db.write_file(filename)
db.close()
def ReceiveCan(qCANDataReceive, ch0, qCANDataReceiveLock):
db = kvadblib.Dbc(filename=r'../CAN/ClubCar_V3_withoutME.dbc')
EPSFeedback = db.get_message_by_name('EPSSteeringAngleFeedback')
# EBSFeedback = db.get_message_by_name('E')
# EPSFeedback = EPSFeedback.bind()
# EPS_02Feedback = db.get_message_by_name('EPS_02')
print('***************************8')
while True:
EPSFeedbackMsg = ch0.read(timeout=1000)
if EPSFeedbackMsg.id == 485:
# bmsg = db.interpret(EPSFeedbackMsg)
# msg = bmsg._message
# EPS_02FeedbackMsg = EPS_02Feedback.bind(ch0.read(timeout = 100))
# print("方向盘转角反馈：%s" % msg.name)
EPSFeedbackMsg = EPSFeedback.bind(EPSFeedbackMsg)
# print("方向盘转角反馈：%s" % EPSFeedbackMsg.EPS_SteeringAngle.phys)
with qCANDataReceiveLock:
if qCANDataReceive.empty():
# print(33333333333333333333)
qCANDataReceive.put([
'EPSFeedBackAngle',
EPSFeedbackMsg.EPS_SteeringAngle.phys
])
# print(11111111111111111111111)
else:
# print(44444444444444444444444444)
while not qCANDataReceive.empty():
# print(555555555555555555)
qCANDataReceive.get(True)
# print(222222222222222222222)
# print(EPSFeedbackMsg)
time.sleep(0.001)
def monitor_channel(channel_number, db_name, bitrate, ticktime):
db = kvadblib.Dbc(filename=db_name)
dbCap = capsule()
ch = canlib.openChannel(channel_number, canlib.canOPEN_ACCEPT_VIRTUAL)
ch.setBusOutputControl(canlib.canDRIVER_NORMAL)
ch.setBusParams(bitrate)
ch.busOn()
timeout = 0.5
tick_countup = 0
if ticktime <= 0:
ticktime = None
elif ticktime < timeout:
timeout = ticktime
print("Listening...")
while True:
try:
frame = ch.read(timeout=int(timeout * 1000))
#printframe(db, frame)
dbCap.db_update(db, frame)
dbcap.pprint()
except canlib.CanNoMsg:
if ticktime is not None:
tick_countup += timeout
while tick_countup > ticktime:
print("tick")
tick_countup -= ticktime
except KeyboardInterrupt:
print("Stop.")
break
except Exception:
pass
def setupCan(self):
try:
self.db = kvadblib.Dbc(filename=__DB_NAME__)
self.dbCap = capsule()
self.ch = canlib.openChannel(__CH_NUM__, canlib.canOPEN_ACCEPT_VIRTUAL)
self.ch.setBusOutputControl(canlib.canDRIVER_NORMAL)
self.ch.setBusParams(__BITRATE__)
self.ch.busOn()
except Exception as ex:
# ROS ERROR LOG
print(ex)
return 0
return 1
def __init__(self):
_dbName_ = "cankingDB_ioniq_dgist_mod6.dbc"
db = kvadblib.Dbc(filename=_dbName_)
__CH_NUM__ = 0
__BITRATE__ = canlib.canBITRATE_500K
rclpy.init()
node = rclpy.create_node(NODENAME)
try:
ch = canlib.openChannel(__CH_NUM__, canlib.canOPEN_ACCEPT_VIRTUAL)
ch.setBusOutputControl(canlib.canDRIVER_NORMAL)
ch.setBusParams(__BITRATE__)
ch.setBusOutputControl(canlib.Driver.NORMAL)
ch.busOn()
except Exception as ex:
print("Error occured :: ", ex, '\nprogram terminated')
return
accelSub = node.create_subscription(Int16,
rootname+subAccel,
self.accelCallback)
angluarSub = node.create_subscription(Int16,
rootname+subAngular,
self.angularCallback)
gearSub = node.create_subscription(Int16,
rootname + subGear,
self.gearCallback)
steerSub = node.create_subscription(Int16,
rootname + subSteer,
self.steerCallback)
brakeSub = node.create_subscription(Int16,
rootname + subBrake,
self.brakeCallback)
self.canSender = alive.sender(ch, db)
print("Node Sub ready")
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
def Save_CAN_Setting(self):
'''加载CAN设备的接口函数链接库'''
'''这里要注意，Dll文件的路径，如果把MainWindow放在工程上层目录的话，这里的就只用./'''
self.CANlib = windll.LoadLibrary('./lib/ControlCAN_Red.dll')
'''加载Kvasaer解析DBC'''
self.DBC = kvadblib.Dbc(filename=self.DBC_Path)
'''设置CAN的通道和波特率'''
self.nCANInd = int(self.CAN_Channel.Channel_Line.text())
self.TIMER0 = int(self.CAN_Channel.Timer0_Line.text(), 16)
self.TIMER1 = int(self.CAN_Channel.Timer1_Line.text(), 16)
'''定义CAN设备结构体vic'''
self.vic = CAN_Par_Set(Acc_Code=0x00000000,
Acc_Mask=0xffffffff,
Filt=0,
BPS1=self.TIMER0,
BPS2=self.TIMER1,
Mod=0)
'''定义行人报文的结构体vco'''
self.vco_Ped = VCI_CAN_OBJ()
'''初始化设备结构函数'''
self.CAN_Device = CAN_Device_Set(nDeviceType=4,
nDeviceInd=0,
nReserved=0,
nCANInd=self.nCANInd)
self.CAN = CAN_Rec(self.CANlib, self.CAN_Device.nDeviceType, self.CAN_Device.nDeviceInd, \
self.CAN_Device.nCANInd, self.CAN_Device.nReserved, self.vic)
'''判断CAN设备是否初始完成并打开，如果打开，将标志位OpenCAN 设置为Yes'''
if self.CAN.CAN_INITIAL_Result()[0] == 1:
self.CAN_Presetting.Line_Ini_CAN.setText(
self.CAN.CAN_INITIAL_Result()[1])
self.CAN_Presetting.Line_Close_CAN.setText('CAN IS OPEN ')
self.OpenCAN = 'Yes'
'''画出摄像头原点'''
self.CAN_FigurePlot.Original()
'''接收行人报文的线程开启,如果CAN设备初始化完成并打开，把线程标志位Thread_Msg_Ped_Rec_Indicator设为'Start'，
同时开启行人报文接收的线程'''
self.Thread_Msg_Ped_Rec_Indicator = 'Start'
'''开启CAN报文接收的线程'''
threading.Thread(target=self.CAN_Msg_Receive).start()
else:
self.CAN_Presetting.Line_Ini_CAN.setText(
self.CAN.CAN_INITIAL_Result()[1])
self.CAN_Presetting.Line_Close_CAN.setText('CAN DEVICE NOT OPEN ')
self.OpenCAN = 'No'
bitrateFlags=canlib.canDRIVER_NORMAL):
ch = canlib.openChannel(channel, openFlags)
print("Using channel: %s, EAN: %s" % (ChannelData(channel).device_name,
ChannelData(channel).card_upc_no)
)
# ch.setBusOutputControl(bitrateFlags)
ch.setBusParams(bitrate)
ch.busOn()
return ch
def tearDownChannel(ch):
ch.busOff()
ch.close()
db = kvadblib.Dbc(filename="../../CAN/CANDB-MASTER-DS1.dbc")
print("canlib version:", canlib.dllversion())
ch0 = setUpChannel(channel=0)
frame = Frame(id_=100, data=[1, 2, 3, 4])#, flags=canlib.canMSG_EXT)
counts = {}
data=0
TCP_IP = '127.0.0.1'
TCP_PORT = 1000
MESSAGE = "a"*36
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
def command():
return -1
#print(value)
self.gear = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
self.ch.write(frame)
time.sleep(0.001)
if __name__ == "__main__":
_dbName_ = "cankingDB_ioniq_dgist_mod6.dbc"
db = kvadblib.Dbc(filename=_dbName_)
__CH_NUM__ = 0
__BITRATE__ = canlib.canBITRATE_500K
try:
ch = canlib.openChannel(__CH_NUM__, canlib.canOPEN_ACCEPT_VIRTUAL)
ch.setBusOutputControl(canlib.canDRIVER_NORMAL)
ch.setBusParams(__BITRATE__)
ch.setBusOutputControl(canlib.Driver.NORMAL)
ch.busOn()
except Exception as ex:
print("Error occured", ex)
test = sender(ch, db)
test.setSteerCMD(500)
def SendCan(qCANDataSend, ch0):
db = kvadblib.Dbc(filename=r'../CAN/ClubCar_V3_withoutME.dbc') # 打开DBC库文件
EPSEnableStatus = 0
EPSInputValue = 0
ThrottleValue = 0
EBSEnableStatus = 0
EBSInputValue = 1
lock = threading.Lock()
def CANDataUpdate():
nonlocal EPSEnableStatus
nonlocal EPSInputValue
nonlocal ThrottleValue
nonlocal EBSEnableStatus
nonlocal EBSInputValue
while True:
lock.acquire()
try:
dataReceive = qCANDataSend.get(True)
if dataReceive[0] == 'EPSInputEnable':
EPSEnableStatus = dataReceive[1]
if dataReceive[0] == 'EPSInput':
EPSInputValue = dataReceive[1]
if dataReceive[0] == 'ThrottleInput':
ThrottleValue = dataReceive[1]
if dataReceive[0] == 'EBSInputEnable':
EBSEnableStatus = dataReceive[1]
if dataReceive[0] == 'EBSInput':
EBSInputValue = dataReceive[1]
finally:
lock.release()
time.sleep(0.01)
def CANSendMessage():
nonlocal EPSEnableStatus
nonlocal EPSInputValue
nonlocal ThrottleValue
nonlocal EBSEnableStatus
nonlocal EBSInputValue
while True:
EPSInput = db.get_message_by_name('EPSInput') # 根据帧的名字获取帧
EPSInput = EPSInput.bind() # 生成一个捆绑报文
EPSInput.EPSMode.phys = EPSEnableStatus
EPSInput.SteeringAngleRequest.phys = EPSInputValue
ThrottleInput = db.get_message_by_name('throttleInput')
ThrottleInput = ThrottleInput.bind()
ThrottleInput.throttle.phys = ThrottleValue
EBSInput = db.get_message_by_name('EBS')
EBSInput = EBSInput.bind()
EBSInput.EBS_brakeEnable.phys = EBSEnableStatus
EBSInput.EBS_brakePadel.phys = EBSInputValue
ch0.write(EPSInput._frame)
ch0.write(ThrottleInput._frame)
ch0.write(EBSInput._frame)
# print(EPSInput)
time.sleep(1 / 50.0) # 50为CAN总线网络控制量的刷新率
# print('发送完成')
t1 = threading.Thread(target=CANDataUpdate)
t2 = threading.Thread(target=CANSendMessage)
t1.start()
t2.start()
t1.join()
t2.join()
ch = canlib.openChannel(channel, openFlags)
print("Using channel: %s, EAN: %s" % (
ChannelData(channel).channel_name, #ChannelData.device_name
ChannelData(channel).card_upc_no))
# ch.setBusOutputControl(bitrateFlags)
ch.setBusParams(bitrate)
ch.busOn()
return ch
def tearDownChannel(ch):
ch.busOff()
ch.close()
db = kvadblib.Dbc(filename="../COSMOS/CanCosmosGenerator/CANDB-MASTER-DS1.dbc")
print("canlib version:", canlib.dllversion())
ch0 = setUpChannel(channel=0)
frame = Frame(id_=100, data=[1, 2, 3, 4]) #, flags=canlib.canMSG_EXT)
counts = {}
data = 0
TCP_IP = '127.0.0.1'
TCP_PORT = 1000
MESSAGE = "a" * 36
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
import matplotlib
import numpy as np
import os
import csv
import math
import copy
import pymap3d as pm
import cubic_spline_planner
from quintic_polynomials_planner import QuinticPolynomial
from multiprocessing import Process, Value
from datetime import datetime
# CAN_library-------------------------
from canlib import canlib, kvadblib
from canlib.canlib import ChannelData
dbc_llh = kvadblib.Dbc(filename='02_GPS_INS_Parser_1206.dbc')
dbc_end = kvadblib.Dbc(filename='06_Path_1027.dbc')
db_lidar = kvadblib.Dbc(filename='05_LiDAR_1005.dbc')
dbc_vel = kvadblib.Dbc(filename='01_Avante_dbc_0710.dbc')
#---------------------------Vehicle----------------------------------
Vehicle_info_2 = dbc_vel.get_message_by_name('Vehicle_Info_2') # -> For Speed
Vehicle_info_2_sig = Vehicle_info_2.bind()
Vehicle_info_4 = dbc_vel.get_message_by_name('Vehicle_info_4') # -> For turn left right
Vehicle_info_4_sig = Vehicle_info_4.bind()
#---------------------------Vehicle----------------------------------
#---------------------------GPS----------------------------------
RTK_GPS_Latitude = dbc_llh.get_message_by_name('RTK_Latitude')
RTK_GPS_Latitude_sig = RTK_GPS_Latitude.bind()
