From 5744ec0892e5b7e993bf64457047a8a08a11e864 Mon Sep 17 00:00:00 2001 From: Varun Ramprakash Date: Thu, 24 Oct 2024 15:06:50 -0500 Subject: [PATCH] reorganized files --- dbw/dbw/dbw_vel.py | 22 +++------ dbw/resource/dbw | 0 dbw/test/{ => old}/test_boundary.py | 0 dbw/test/{ => old}/test_boundry_output.py | 0 .../test_boundry_output_btye_slip.py | 0 dbw/test/{ => old}/test_ouput_range.py | 0 dbw/test/{ => old}/test_uint8_steering.py | 0 dbw/test/{ => old}/test_uint8_throttle.py | 0 ...test_merge_bytes.py => test_byte_merge.py} | 2 +- dbw/test/test_byte_split.py | 28 +++++++---- dbw/test/test_hex_to_vel.py | 47 +++++++++++++++++++ 11 files changed, 74 insertions(+), 25 deletions(-) delete mode 100644 dbw/resource/dbw rename dbw/test/{ => old}/test_boundary.py (100%) rename dbw/test/{ => old}/test_boundry_output.py (100%) rename dbw/test/{ => old}/test_boundry_output_btye_slip.py (100%) rename dbw/test/{ => old}/test_ouput_range.py (100%) rename dbw/test/{ => old}/test_uint8_steering.py (100%) rename dbw/test/{ => old}/test_uint8_throttle.py (100%) rename dbw/test/{test_merge_bytes.py => test_byte_merge.py} (94%) create mode 100644 dbw/test/test_hex_to_vel.py diff --git a/dbw/dbw/dbw_vel.py b/dbw/dbw/dbw_vel.py index 653c095..8f278a9 100644 --- a/dbw/dbw/dbw_vel.py +++ b/dbw/dbw/dbw_vel.py @@ -1,7 +1,6 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Float32MultiArray from dbw_msgs.msg import Dbw import numpy as np @@ -14,9 +13,6 @@ def __init__(self): # init subscriber self.dbw_sub = self.create_subscription(Dbw, '/control_cmd', self.dbw_callback, 10) - # init publisher - self.vcu_pub = self.create_publisher(Float32MultiArray, '/vcu_data', 10) - # init timer self.timer = self.create_timer(0.1, self.timer_callback) @@ -61,11 +57,7 @@ def timer_callback(self): self.bus.send(self.heartBeat) self.count = 0 - vcu_msg = Float32MultiArray() - vcu_msg.data = [] - self.vcu_pub.publish(vcu_msg) - - def set_2_bytes_number(self, byte_list, number, index): + def split_2_bytes(self, byte_list, number, index): """Sets a 2-byte number in a list of bytes. Args: @@ -110,22 +102,22 @@ def dbw_callback(self, msg): else: print('gear: neutral') throttle_cmd = np.uint16(self.lin_offset) - + steering_cmd = np.uint16(self.ang_spd_range * (steering_percentage + self.ang_percent_offset) / self.ang_percent_range) # throttle - self.set_2_bytes_number(self.propulsion.data, throttle_cmd, 1) - + self.split_2_bytes(self.propulsion.data, throttle_cmd, 1) + # steering - self.set_2_bytes_number(self.propulsion.data, steering_cmd, 3) + self.split_2_bytes(self.propulsion.data, steering_cmd, 3) if inhibit == 1: self.inhibitCmd.data[4] = 0x10 else: self.inhibitCmd.data[4] = 0x00 - + self.bus.send(self.propulsion) - + print('sending control command') def main(): diff --git a/dbw/resource/dbw b/dbw/resource/dbw deleted file mode 100644 index e69de29..0000000 diff --git a/dbw/test/test_boundary.py b/dbw/test/old/test_boundary.py similarity index 100% rename from dbw/test/test_boundary.py rename to dbw/test/old/test_boundary.py diff --git a/dbw/test/test_boundry_output.py b/dbw/test/old/test_boundry_output.py similarity index 100% rename from dbw/test/test_boundry_output.py rename to dbw/test/old/test_boundry_output.py diff --git a/dbw/test/test_boundry_output_btye_slip.py b/dbw/test/old/test_boundry_output_btye_slip.py similarity index 100% rename from dbw/test/test_boundry_output_btye_slip.py rename to dbw/test/old/test_boundry_output_btye_slip.py diff --git a/dbw/test/test_ouput_range.py b/dbw/test/old/test_ouput_range.py similarity index 100% rename from dbw/test/test_ouput_range.py rename to dbw/test/old/test_ouput_range.py diff --git a/dbw/test/test_uint8_steering.py b/dbw/test/old/test_uint8_steering.py similarity index 100% rename from dbw/test/test_uint8_steering.py rename to dbw/test/old/test_uint8_steering.py diff --git a/dbw/test/test_uint8_throttle.py b/dbw/test/old/test_uint8_throttle.py similarity index 100% rename from dbw/test/test_uint8_throttle.py rename to dbw/test/old/test_uint8_throttle.py diff --git a/dbw/test/test_merge_bytes.py b/dbw/test/test_byte_merge.py similarity index 94% rename from dbw/test/test_merge_bytes.py rename to dbw/test/test_byte_merge.py index 3691517..7fb7b75 100644 --- a/dbw/test/test_merge_bytes.py +++ b/dbw/test/test_byte_merge.py @@ -19,7 +19,7 @@ print() -def merge_bytes(high_byte, low_byte): +def merge_2_bytes(high_byte, low_byte): high = high_byte*2**8 + 0xFF low = 0xFF00 + low_byte diff --git a/dbw/test/test_byte_split.py b/dbw/test/test_byte_split.py index 33aa53c..070af77 100644 --- a/dbw/test/test_byte_split.py +++ b/dbw/test/test_byte_split.py @@ -1,15 +1,25 @@ # This test aims to check the byte split function which splits a given 2-byte (16-bit) value into 2 separate bytes (8 bits each). +import numpy as np -for i in range(8): - number = int(65535/7 * i) - print("Number in hexadecimal: ", format(number, 'x')) - print("Number in binary: ", format(number, 'b')) - index = 0 - byte_list = [0,0] - byte_list[index] = number & 0xFF # Low byte - byte_list[index + 1] = (number >> 8) & 0xFF # High byte +def split_2_bytes(byte_list, number, index): + """Sets a 2-byte number in a list of bytes. + Args: + byte_list (list): The list of bytes. + number (int): The 2-byte number to set. + index (int): The starting index in the list to set the number. + """ + if number < 0 or number > 65535: + raise ValueError("Number must be between 0 and 65535") + byte_list[index] = number & 0xFF # Low byte + byte_list[index + 1] = (number >> 8) & 0xFF # High byte print("High byte: ", "{0:b}".format(byte_list[1]), "{0:x}".format(byte_list[1])) print("Low byte: " , "{0:b}".format(byte_list[0]), "{0:x}".format(byte_list[0])) - print() \ No newline at end of file +for i in range(8): + number = np.uint16(65535/7 * i) + print("Number in hexadecimal: ", format(number, 'x')) + print("Number in binary: " , format(number, 'b')) + index = 0 + byte_list = [0,0] + split_2_bytes(byte_list, number, index) \ No newline at end of file diff --git a/dbw/test/test_hex_to_vel.py b/dbw/test/test_hex_to_vel.py new file mode 100644 index 0000000..117c751 --- /dev/null +++ b/dbw/test/test_hex_to_vel.py @@ -0,0 +1,47 @@ +import numpy as np + +lin_vel_step_size = 1/128 +lin_vel_min = -250 # km/hr +lin_vel_eff_max = 40 # km/h + +ang_vel_step_size = 1/1024 +ang_vel_min = -31.25 # rad/s +ang_vel_eff_max = 31.25 # rad/s + +lin_percent_range = 100.0 +ang_percent_range = 200.0 +ang_percent_offset = 100.0 + +lin_offset = abs(lin_vel_min / lin_vel_step_size) +lin_spd_range = lin_vel_eff_max / lin_vel_step_size + +ang_offset = abs(ang_vel_min / ang_vel_step_size) +ang_spd_range = (ang_vel_eff_max - ang_vel_min) / ang_vel_step_size + +throttle_cmd_new = [] +steering_cmd_new = [] + +for i in range(21): + throttle_percentage = 100/20 * i + steering_percentage = 200/20 * i - 100 + + gear = i%3 + + if gear == 1: + # print('gear: forward') + throttle_cmd = np.uint16(lin_spd_range * throttle_percentage / lin_percent_range + lin_offset) + throttle_cmd_new.append(throttle_cmd) + elif gear == 2: + # print('gear: backward') + throttle_cmd = np.uint16(lin_spd_range * -throttle_percentage / lin_percent_range + lin_offset) + throttle_cmd_new.append(throttle_cmd) + else: + # print('gear: neutral') + throttle_cmd = np.uint16(lin_offset) + throttle_cmd_new.append(throttle_cmd) + + steering_cmd = np.uint16(ang_spd_range * (steering_percentage + ang_percent_offset) / ang_percent_range) + steering_cmd_new.append(steering_cmd) + +print(throttle_cmd_new) +print(steering_cmd_new) \ No newline at end of file