def __init__(self, channel=0):
self.channel = channel
self.openFlags = canlib.canOPEN_ACCEPT_VIRTUAL
self.bitrate = canlib.canBITRATE_500K
self.bitrateFlags = canlib.canDRIVER_NORMAL
self.valid = False
self.ch = None
self.device_name = ''
self.card_upc_no = ''
try:
ch = canlib.openChannel(self.channel, self.openFlags)
ch.setBusOutputControl(self.bitrateFlags)
ch.setBusParams(self.bitrate)
ch.iocontrol.timer_scale = 1
ch.iocontrol.local_txecho = True
ch.busOn()
except exceptions.CanGeneralError:
self.valid = False
self.ch = None
else:
self.valid = True
self.ch = ch
self.device_name = ChannelData.channel_name
self.card_upc_no = ChannelData(self.channel).card_upc_no
if self.card_upc_no == '00-00000-00000-0':
print('Virtual CAN 모드입니다.')
def __init__(self, bitrate=1000000, channel=0):
super().__init__(bitrate)
# reinit library to clearup any previous connections
canlib.reinitializeLibrary()
# create the channel
self.ch = canlib.openChannel(channel,
bitrate=bitrates[bitrate],
flags=canlib.Open.EXCLUSIVE)
# set the device mode to normal (sends ACKs)
self.ch.setBusOutputControl(canlib.Driver.NORMAL)
# clear the recieve buffer
self.ch.iocontrol.flush_rx_buffer()
# filter out heartbeats as they seem to screw up the buffer
self.ch.canSetAcceptanceFilter(0x080, 0x080)
# thread for fetching messages form device
self.read_thread = threading.Thread(target=self._read_loop)
# variable to indicate if thread is running. clear to stop it
self.reading = threading.Event()
self.reading.clear()
def connect(self):
self.channel = self.config.get("KV_CHANNEL")
openFlags = canlib.canOPEN_ACCEPT_VIRTUAL if self.config.get(
"KV_ACCEPT_VIRTUAL") == True else None
bitrate = canlib.canBITRATE_500K
#bitrateFlags = canlib.canDRIVER_NORMAL
self.ch = canlib.openChannel(self.channel, openFlags)
self.parent.logger.debug("{} [CANLib version: {}]".format(
ChannelData(self.channel).device_name, canlib.dllversion()))
baudrate = int(self.parent.config.get("BAUDRATE"))
if self.config.get("KV_BAUDRATE_PRESET"):
if not baudrate in BAUDRATE_PRESETS:
raise ValueError(
"No preset for baudrate '{}'".format(baudrate))
self.ch.setBusParams(BAUDRATE_PRESETS[baudrate])
else:
samplePoint = self.config.get("SAMPLE_POINT")
sjw = self.config.get("SJW")
tseg1 = self.config.get("TSEG1")
tseg2 = self.config.get("TSEG2")
self.ch.setBusParams(baudrate, tseg1, tseg2, sjw)
self.ch.iocontrol.timer_scale = 10 # 10µS, fixed for now.
self.ch.busOn()
self.connected = True
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 setUpChannel(channel, openFlags=canlib.canOPEN_ACCEPT_VIRTUAL, bitrate=canlib.canBITRATE_500K, 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 connect_can_bus(cls):
# Open CAN channel, virtual channels are considered ok to use
# Todo: put usb port in config
usb_port = 0
if cls._hw_interface_can_bus is None:
cls._hw_interface_can_bus = canlib.openChannel(usb_port, canlib.canOPEN_ACCEPT_VIRTUAL)
logger.debug("Setting bit rate to 1000 kb/s")
cls._hw_interface_can_bus.setBusParams(canlib.canBITRATE_1M)
logger.debug("Going on bus\n\n")
cls._hw_interface_can_bus.busOn()
cls._using_count += 1
def init(self, parent, master_id_with_ext: int, slave_id_with_ext: int,
receive_callback):
self.parent = parent
bitrate = canlib.canBITRATE_500K
#bitrateFlags = canlib.canDRIVER_NORMAL
self.ch = canlib.openChannel(self.channel, self.openFlags)
self.parent.logger.debug("{} [CANLib version: {}]".format(
ChannelData(self.channel).device_name, canlib.dllversion()))
self.ch.setBusParams(canlib.canBITRATE_250K)
#self.ch.setBusOutputControl(bitrateFlags)
self.ch.iocontrol.timer_scale = 10 # 10µS, fixed for now.
def setUpChannel(channel=0,
openFlags=canlib.Open.ACCEPT_VIRTUAL,
bitrate=canlib.canBITRATE_1M,
outputControl=canlib.Driver.NORMAL):
ch = canlib.openChannel(channel, openFlags)
print("Using channel: %s, EAN: %s" % (
canlib.ChannelData(channel).channel_name,
canlib.ChannelData(channel).card_upc_no))
ch.setBusOutputControl(outputControl)
ch.setBusParams(bitrate)
ch.busOn()
return ch
def set_up_channel(chan):
"""
Opens a Kvaser CAN channel
"""
channel = canlib.openChannel(chan,
flags=canlib.Open.ACCEPT_VIRTUAL,
bitrate=canlib.canBITRATE_500K)
channel.setBusOutputControl(canlib.Driver.NORMAL)
channel.busOn()
return channel
def __init__(self,
channel,
message_id=0x7FF,
bit_rate=250000,
sampling_point=75): # private
from canlib import canlib
self.__message_id = message_id
bs1, bs2 = get_can_settings(sampling_point)
self.__can = canlib.openChannel(channel=channel)
self.__can.setBusParams(freq=bit_rate, tseg1=bs1, tseg2=bs2, sjw=1)
self.__can.canSetAcceptanceFilter(code=message_id, mask=0x3FF)
self.__can.busOn()
rpc_slave.__init__(self)
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 initialize(self):
while True:
try:
self.channel = 0
self.ch = canlib.openChannel(self.channel,
canlib.canOPEN_ACCEPT_VIRTUAL)
self.ch.setBusOutputControl(canlib.canDRIVER_NORMAL)
self.ch.setBusParams(canlib.canBITRATE_500K)
self.ch.busOn()
t1 = Thread(target=self.work, args=())
t1.daemon = True
t1.start()
while t1.is_alive():
time.sleep(1)
except Exception as e:
print('Error creating CAN Channel.... Retrying')
def set_channelConnection(
channel=0,
openFlags=canlib.Open.ACCEPT_VIRTUAL,
bitrate=111111, #canlib.canBITRATE_125K,
outputControl=canlib.Driver.NORMAL,
sjw=4,
tseg1=5,
tseg2=6):
ch = canlib.openChannel(channel, openFlags)
print("Using channel: %s, EAN: %s" %
(canlib.ChannelData(channel).device_name,
canlib.ChannelData(channel).card_upc_no))
ch.setBusOutputControl(outputControl)
ch.setBusParams(freq=bitrate, sjw=sjw, tseg1=tseg1, tseg2=tseg2)
ch.busOn()
return ch
def openChannel(channel=0,
openFlags=canlib.canOPEN_ACCEPT_VIRTUAL,
bitrate=canlib.canBITRATE_500K,
bitrateFlags=canlib.canDRIVER_NORMAL):
# cl = canlib.canlib()
ch = canlib.openChannel(channel, openFlags)
num_channels = canlib.getNumberOfChannels()
print("num_channels:%d" % num_channels)
print(
"%d. %s (%s / %s)" % (num_channels, canlib.ChannelData(0).channel_name,
canlib.ChannelData(0).card_upc_no,
canlib.ChannelData(0).card_serial_no))
ch.setBusOutputControl(bitrateFlags)
ch.setBusParams(bitrate)
ch.busOn()
# print("somethin wrong***************************************")
return ch
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 init(channel_number=0, bitrate=500):
channel_number = 0
# Specific CANlib channel number may be specified as first argument
if len(sys.argv) == 2:
channel_number = int(sys.argv[1])
print("Opening channel %d" % (channel_number))
# Use ChannelData to get some information about the selected channel
chd = canlib.ChannelData(channel_number)
print("%d. %s (%s / %s) " % (channel_number, chd.channel_name,
chd.card_upc_no, chd.card_serial_no))
# Open CAN channel, virtual channels are considered ok to use
ch = canlib.openChannel(channel_number, canlib.canOPEN_ACCEPT_VIRTUAL)
ch.iocontrol.local_txecho = False
print("Setting bitrate to 500 kb/s")
ch.setBusParams(canlib.canBITRATE_500K)
# print("Going on bus")
# ch.busOn()
return ch
def main():
# Initialize ROS node
rospy.init_node('control')
# Read parameters from ROS parameter server
tout = rospy.get_param('~timeout', 150)
chan = rospy.get_param('~channel', 0)
# Open Kvaser CAN channel
channel = canlib.openChannel(chan,
flags=canlib.Open.ACCEPT_VIRTUAL,
bitrate=canlib.canBITRATE_500K)
channel.setBusOutputControl(canlib.Driver.NORMAL)
channel.busOn()
def callback(msg):
"""
Publishes control signals to CAN bus
"""
angle = msg.angle * 180 / math.pi # rad to degrees
speed = msg.speed * 3.6 # m/s to km/h
# Prepare and write reference steering angle data
angle_data = [
int(min(round(abs(angle) * 255.0 / 40), 255)),
0 if angle < 0 else 255
]
channel.writeWait_raw(id_=150, msg=angle_data, dlc=8, timeout=tout)
# Prepare and write reference speed
speed_data = [
int(min(round(abs(speed) * 255.0 / 5), 255)),
0 if speed > 0 else 255
]
channel.writeWait_raw(id_=154, msg=speed_data, dlc=8, timeout=tout)
# Subscribe to topic where reference control values will be published
rospy.Subscriber('twizy_control', CarControl, callback, queue_size=1)
# Wait for shutdown
rospy.spin()
def __init__(self):
# Initialize existence of thread, declare canlib object, open a channel handle (0), set channel parameters
QtCore.QThread.__init__(self)
# Create canlib object
# Print out current canlib driver version to console
print("CANlib version: " + str(canlib.dllversion()))
# Open a channel that accepts connected physical CAN device, default to virtual channel if none found
self.handle1 = canlib.openChannel(0, canlib.canOPEN_ACCEPT_VIRTUAL)
self.h1data = canlib.ChannelData(0)
# Print out current channel name and data to console
print("Using channel: " + str(self.h1data.channel_name) + ", EAN: " +
str(self.h1data.card_upc_no))
# Set the can bus control to normal
self.handle1.setBusOutputControl(canlib.canDRIVER_NORMAL)
# Set CAN bit-rate to 125k (GCM default bit-rate)
self.handle1.setBusParams(canlib.canBITRATE_125K)
# Turn channel bus ON
self.handle1.busOn()
self.actively_dumping = True # Flag for 'self.run' method
def set_channel_connection(self, interface=None):
"""
Set the internal attribute for the |CAN| channel
The function is important to initialise the channel
Parameters
----------
interface: String
"""
self.logger.notice('Going in \'Bus On\' state ...')
try:
if interface == 'Kvaser':
self.__ch = canlib.openChannel(int(self.__channel),
canlib.canOPEN_ACCEPT_VIRTUAL)
self.__ch.setBusOutputControl(
canlib.Driver.NORMAL) # New from tutorial
self.__ch.setBusParams(freq=int(self.__bitrate),
sjw=int(self.__sjw),
tseg1=int(self.__tseg1),
tseg2=int(self.__tseg2))
self.__ch.busOn()
self.__canMsgThread = Thread(
target=self.read_can_message_thread)
elif interface == 'AnaGate':
self.__ch = analib.Channel(ipAddress=self.__ipAddress,
port=self.__channel,
baudrate=self.__bitrate)
elif interface == 'virtual':
channel = "vcan" + str(self.__channel)
self.__ch = can.interface.Bus(bustype="socketcan",
channel=channel)
else:
channel = "can" + str(self.__channel)
self.__ch = can.interface.Bus(bustype=interface,
channel=channel,
bitrate=self.__bitrate)
except Exception:
self.logger.error("TCP/IP or USB socket error in %s interface" %
interface)
self.logger.success(str(self))
def __init__(self):
__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:
node.get_logger().fatal(
"dbw_cmd_node : CAN Device Connect Error!!")
print("Error occured :: ", ex, '\nprogram terminated')
return
node.get_logger().info("dbw_cmd_node : CAN Device Connect Success")
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 = can_communication.sender(ch, node)
print("Node Sub ready")
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
def start_listening_channel(self,
ch,
flags=canlib.canOPEN_ACCEPT_VIRTUAL,
bit_rate=canlib.canBITRATE_250K):
try:
self.channel = canlib.openChannel(ch, flags)
self.channel.setBusParams(bit_rate)
self.channel.busOn()
# Set node to operational state
self.__send_start_signal()
self.__set_angle_data_interval(10)
# Start thread for listen client messages
if self.listenClient:
self.clientMsgListener.start()
# Start listening for messages
self.__dump_message_loop()
except canlib.exceptions.CanNotFound as ex:
print(ex)
# Specific CANlib channel number may be specified as first argument
if len(sys.argv) == 2:
channel_number = int(sys.argv[1])
print("Opening channel %d" % (channel_number))
# Use ChannelData to get some information about the selected channel
chd = canlib.ChannelData(channel_number)
print("%d. %s (%s / %s) " %
(channel_number, chd.channel_name, chd.card_upc_no, chd.card_serial_no))
# If the channel have a custom name, print it
# if chd.custom_name != '':
# print("Customized Channel Name: %s " % (chd.custom_name))
# Open CAN channel, virtual channels are considered ok to use
ch = canlib.openChannel(channel_number, canlib.canOPEN_ACCEPT_VIRTUAL)
#Add by hzc
ch.iocontrol.local_txecho = False
print("Setting bitrate to 500 kb/s")
ch.setBusParams(canlib.canBITRATE_500K)
print("Going on bus")
ch.busOn()
#******* Add by HZC ******#
# print("Sending a message")
# for j in range(10):
# frame = Frame(id_=0x79D,
# data=[0x02,0x10, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00],
def __init__(self,
channel=None,
bitrate=canlib.canBITRATE_125K,
nodeId=None,
loglevel=None,
logdir=None,
logformat='%(asctime)s %(levelname)-8s %(message)s'):
if channel is None:
channel = 0
if nodeId is None:
nodeId = 42
if loglevel is None:
loglevel = logging.NOTICE
self.__state = 0
# Initialize logging
extend_logging()
verboselogs.install()
self.logger = logging.getLogger(os.path.basename(__name__))
self.logger.setLevel(logging.DEBUG)
if logdir is None:
scrdir = os.path.dirname(os.path.abspath(__file__))
else:
scrdir = logdir
self.canLogger = logging.getLogger('CAN_messages')
self.canLogger.setLevel(logging.DEBUG)
fname = os.path.join(scrdir, 'log', strftime('%Y-%m-%d_%H-%M-%S_'))
self.__fh = RotatingFileHandler(fname + 'DCSController.log',
backupCount=10,
maxBytes=10 * 1024 * 1024)
self.__cfh = RotatingFileHandler(fname + 'CANmsg.log',
backupCount=10,
maxBytes=10 * 1024 * 1024)
fmt = logging.Formatter(logformat)
fmt.default_msec_format = '%s.%03d'
self.__fh.setFormatter(fmt)
self.__cfh.setFormatter(fmt)
cl.install(fmt=logformat, level=loglevel, isatty=True)
self.__fh.setLevel(logging.DEBUG)
self.__cfh.setLevel(logging.DEBUG)
self.logger.addHandler(self.__fh)
self.canLogger.addHandler(self.__cfh)
self.canLogger.info(coc.MSGHEADER)
# Intialize object dictionary
fp = os.path.join(scrdir, 'CANControllerForPSPPv1.eds')
self.__od = od.from_eds(self.logger, fp, 42, True)
for scb in range(4):
val = 0
if scb == 0:
val = 0
elif scb == 1:
val = 0b1111
elif scb == 2:
val = 0b11
elif scb == 3:
val = 0
# val = rdm.randrange(2**16)
self.logger.notice(f'Connected PSPPs on SCB {scb}: {val:016b}')
self.__od[0x2000][1 + scb].value = val
states = [bool(int(x)) for x in f'{val:016b}'[::-1]]
for pspp in range(16):
index = 0x2200 | (scb << 4) | pspp
self.__od[index][2].value = states[pspp]
if states[pspp]:
for reg in range(13):
self.__od[index][0x10 | reg].value = 0
self.__od[index][0x10].value = 0x21
self.__od[index][0x11].value = 0xF5
# Initialize library and open channel
self.__channel = channel
self.__bitrate = bitrate
self.__nodeId = nodeId
self.__toggleBit = False
self.__ch = canlib.openChannel(channel, canlib.canOPEN_ACCEPT_VIRTUAL)
self.logger.success(str(self))
self.__state = 127
self.__ch.setBusParams(bitrate)
self.__ch.busOn()
self.logger.success('You are in \'Bus On\' state!')
#print(value)
self.gear = value
frame = Frame(id_=0x152,
data=(self.frame152()).to_bytes(8,
byteorder="little",
signed=False))
try:
self.ch.write(frame)
except:
self.node.get_logger().fatal("dbw_cmd_node : can send failed")
time.sleep(0.001)
if __name__ == "__main__":
__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)
test.setSteerCMD(500)
# 01_canlib_buson_off.py
from canlib import canlib
from canlib.canlib import ChannelData
channel = 0
chd = canlib.ChannelData(channel)
print("CANlib version: v{}".format(chd.dll_product_version))
print("canlib dll version: v{}".format(chd.dll_file_version))
print("Using channel: {ch}, EAN: {ean}".format(ch=chd.device_name,
ean=chd.card_upc_no))
ch1 = canlib.openChannel(channel, canlib.canOPEN_ACCEPT_VIRTUAL)
ch1.setBusOutputControl(canlib.canDRIVER_NORMAL)
ch1.setBusParams(canlib.canBITRATE_1M)
ch1.busOn()
ch1.busOff()
