diff --git a/dbw/dbw/can_test.py b/dbw/dbw/can_test.py deleted file mode 100755 index de9c1d2..0000000 --- a/dbw/dbw/can_test.py +++ /dev/null @@ -1,87 +0,0 @@ -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String, Float32MultiArray -from dbw_msgs.msg import Dbw - -import numpy as np - -from ctypes import * -import ctypes - - -# my_func = CDLL("/home/neousys/Workspace/src/dbw/dbw/C_library/helper.so") -neo_func = CDLL("/home/neousys/Workspace/src/dbw/dbw/C_library/libwdt_dio.so") - -class CAN_SETUP(ctypes.Structure): - _fields_ = [ - ("bitRate", ctypes.c_uint32), - ("recvConfig", ctypes.c_uint32), - ("recvId", ctypes.c_uint32), - ("recvMask", ctypes.c_uint32) - ] - -class CAN_MSG(ctypes.Structure): - _fields_ = [ - ("id", ctypes.c_uint32), - ("flags", ctypes.c_uint16), - ("len", ctypes.c_uint8), - ("data", ctypes.c_uint8*8) - ] - -class DBW(Node): - def __init__(self): - super().__init__('dbw') - - # init timer - self.timer = self.create_timer(1, self.timer_callback) - self.i = 0 - print("get version") - - print(neo_func.MCU_GetVersion(0x20150325)) - - # self.can_setup = CAN_SETUP() - # self.can_setup.bitRate = 250000 # 250kbps - # self.can_setup.recvConfig = 0x00000008 - # self.can_setup.recvId = 0 - # self.can_setup.recvMask = 0 - - # print(neo_func.CAN_Setup(0, self.can_setup, sizeof(self.can_setup))) - # print("Setup finished") - - # self.tx_msg = CAN_MSG() - # self.tx_msg.id = 0x1A0 - # self.tx_msg.len = 8 - - # self.tx_msg.data[0] = 0 - # self.tx_msg.data[1] = 1 - # self.tx_msg.data[2] = 2 - # self.tx_msg.data[3] = 3 - # self.tx_msg.data[4] = 4 - # self.tx_msg.data[5] = 5 - # self.tx_msg.data[6] = 6 - # self.tx_msg.data[7] = 7 - - - def timer_callback(self): - # print(my_func.square(self.i)) - self.i += 1 - # print(neo_func.CAN_Send(0, self.tx_msg, sizeof(self.tx_msg))) - - - - - - - -def main(): - rclpy.init() - dbw_node = DBW() - rclpy.spin(dbw_node) - neo_func.CAN_Stop(0) - dbw_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/dbw/dbw/can_test_2.py b/dbw/dbw/can_test_2.py deleted file mode 100755 index 19d404f..0000000 --- a/dbw/dbw/can_test_2.py +++ /dev/null @@ -1,44 +0,0 @@ -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String, Float32MultiArray -from dbw_msgs.msg import Dbw - -import numpy as np - -from ctypes import * -import ctypes - - -my_func = CDLL("/home/neousys/Workspace/src/dbw/dbw/C_library/neo_can.so") - - -class DBW(Node): - def __init__(self): - super().__init__('dbw') - - # init timer - self.timer = self.create_timer(1, self.timer_callback) - - - - def timer_callback(self): - print(my_func.can()) - - - - - - - - -def main(): - rclpy.init() - dbw_node = DBW() - rclpy.spin(dbw_node) - dbw_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/dbw/dbw/dbw_neo.py b/dbw/dbw/dbw_neo.py deleted file mode 100644 index ae6f829..0000000 --- a/dbw/dbw/dbw_neo.py +++ /dev/null @@ -1,101 +0,0 @@ -import rclpy -from rclpy.node import Node - -from std_msgs.msg import String, Float32MultiArray -from dbw_msgs.msg import Dbw - -import numpy as np -from ctypes import * - - -so_file = "/home/neousys/Workspace/src/dbw/dbw/C_library/neo_can.so" -neo_func = CDLL(so_file) - -class DBW(Node): - def __init__(self): - super().__init__('dbw') - - # init subscriber - self.dbw_sub = self.create_subscription(Dbw, '/control_cmd', self.dbw_callback, 10) - - # init timer - self.timer = self.create_timer(0.1, self.timer_callback) - - # init can - # print(neo_func.square(3)) - # print(neo_func.can_init()) - - # init varibables - self.count = 0 - - # can messages definations - # sent messages - # self.teleop = can.Message(arbitration_id=0x18FFFF2A, data=[0x82, 0x7D, 0x00, 0x7D, 0xFF, 0xFF, 0xFF, 0xFF], is_extended_id=True) - # self.addressClaim = can.Message(arbitration_id=0x18EEFF2A, data=[0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80], is_extended_id=True) - # self.heartBeat = can.Message(arbitration_id=0x18FFFF2A, data=[0x80, 0x31, 0x5D, 0x55, 0xFF, 0xFD, 0xFF, 0xFF], is_extended_id=True) - # self.inhibitCmd = can.Message(arbitration_id=0x0CFE5A2A, data=[0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF], is_extended_id=True) - - - # send address claim message - # self.bus.send(self.addressClaim) - - def timer_callback(self): - neo_func.can_run() - # print("timer working") - # self.bus.send(self.inhibitCmd) - - # self.count += 1 - # if self.count == 10: - # self.bus.send(self.heartBeat) - # self.count = 0 - - - def dbw_callback(self, msg): - inhibit = msg.parkbrake # 1 for lock; 0 for unlock - gear = msg.gear # 0 for neutral; 1 for forward; 2 for backward - throttle_percentage = msg.throttle # 0 ~ 100 (%) - brake_percentage = msg.brake # 0 ~ 100 (%) - steering_percentage = msg.steering # -100 ~ 100 (%), left+, right- - # steering_percentage = 50.0 - - if gear == 1: - print('gear: forward') - throttle_cmd = hex(int(250.0*(throttle_percentage+100)/200.)) - elif gear == 2: - print('gear: backward') - throttle_cmd = hex(int(250.0 * (100.0 - throttle_percentage) / 200.)) - else: - print('gear: neutral') - throttle_cmd = hex(int(250.0 * (0 + 100) / 200.)) - - brake_cmd = hex(int(250.0*brake_percentage/100.)) - steering_cmd = hex(int(250.0*(steering_percentage+100)/200.)) - - - self.teleop.data[1] = np.uint8(int(throttle_cmd, 16)) - # throttle - - self.teleop.data[2] = np.uint8(int(brake_cmd, 16)) - # brake - - self.teleop.data[3] = np.uint8(int(steering_cmd, 16)) - # steering - - if inhibit == 1: - self.inhibitCmd.data[4] = 0x10 - else: - self.inhibitCmd.data[4] = 0x00 - - # self.bus.send(self.teleop) - print('sending control command') - -def main(): - rclpy.init() - dbw_node = DBW() - rclpy.spin(dbw_node) - dbw_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/dbw/dbw/test.py b/dbw/dbw/test.py deleted file mode 100644 index ecd73ff..0000000 --- a/dbw/dbw/test.py +++ /dev/null @@ -1,43 +0,0 @@ -import can -from can.message import Message - -# class MyListener(can.Listener): -# def __init__(self): -# super(MyListener, self).__init__() - -# def on_message_received(self, msg: Message) -> None: -# """ -# example -# when receive message 0x123，transmit message 0x456 -# """ -# if msg.arbitration_id == 0x0CF00400: -# print("CAN msg received") - - - -def print_msg(msg): - if msg.arbitration_id == 0x0CF00400: - print("CAN msg received") - print(msg) - - -if __name__ == "__main__": - # bus = can.interface.Bus('virtual_ch', bustype='virtual') - # bus = can.interface.Bus(channel='can0', bustype='socketcan') - bus = can.Bus(interface='socketcan', channel='can0', bitrate=250000) - # logger = can.Logger("logfile.asc") # save log to asc file - listeners = [ - print_msg, # Callback function, print the received messages - # logger, # save received messages to asc file - # MyListener # my listener - ] - notifier = can.Notifier(bus, listeners) - running = True - while running: - input() - running = False - - # It's important to stop the notifier in order to finish the writting of asc file - notifier.stop() - # stops the bus - bus.shutdown() diff --git a/dbw/setup.py b/dbw/setup.py index 4aab9d5..85b6aad 100644 --- a/dbw/setup.py +++ b/dbw/setup.py @@ -20,10 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'dbw_neo = dbw.dbw_neo:main', - 'dbw_read = dbw.dbw_read:main', - 'can_test = dbw.can_test:main', - 'can_test_2 = dbw.can_test_2:main' + 'dbw_read = dbw.dbw_read:main' ], }, ) diff --git a/vectornav b/vectornav deleted file mode 160000 index aa89067..0000000 --- a/vectornav +++ /dev/null @@ -1 +0,0 @@ -Subproject commit aa890673b5e56165d20a9c137120542ca6a36e1e