Skip to content

Commit

Permalink
reorganized files
Browse files Browse the repository at this point in the history
  • Loading branch information
varunrpj committed Oct 24, 2024
1 parent 2a15ec2 commit 5744ec0
Showing 11 changed files with 74 additions and 25 deletions.
22 changes: 7 additions & 15 deletions dbw/dbw/dbw_vel.py
Original file line number Diff line number Diff line change
@@ -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():
Empty file removed dbw/resource/dbw
Empty file.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
Original file line number Diff line number Diff line change
@@ -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

28 changes: 19 additions & 9 deletions dbw/test/test_byte_split.py
Original file line number Diff line number Diff line change
@@ -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()
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)
47 changes: 47 additions & 0 deletions dbw/test/test_hex_to_vel.py
Original file line number Diff line number Diff line change
@@ -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)

0 comments on commit 5744ec0

Please sign in to comment.