diff --git a/.gitignore b/.gitignore index a428706..a49e18b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,5 @@ ros/build ros/devel -<<<<<<< HEAD .vscode -======= *.swp ->>>>>>> origin/surface-hunter +.idea/* diff --git a/README.md b/README.md index ece08ca..49bfc77 100644 --- a/README.md +++ b/README.md @@ -136,4 +136,6 @@ After what seems like an eternity (about 30 minutes for me), you should have a w If this was ported from X12, some files still contain python shebangs. Try changing them to python3 ones. Another issue might be is that you need to build nodes in `/ros` before `/surface/ros` -In all cases, especially if you have had previous ROS installs on your machine, make sure `rosdep init`'s file is up-to-date with Python 3 dependencies. Meaning run `rosdep init` to find the file and delete it. Run that command again and run `rosdep update`. You may need to use `sudo` with the init stage. \ No newline at end of file +In all cases, especially if you have had previous ROS installs on your machine, make sure `rosdep init`'s file is up-to-date with Python 3 dependencies. Meaning run `rosdep init` to find the file and delete it. Run that command again and run `rosdep update`. You may need to use `sudo` with the init stage. + +Updated: 9/29/21 diff --git a/ros/.gitignore b/ros/.gitignore index c07bf59..bcaedda 100644 --- a/ros/.gitignore +++ b/ros/.gitignore @@ -1,3 +1,4 @@ .catkin_workspace -*.swp +**/*.swp **/__pycache__ +**/*.pyc diff --git a/ros/compros.sh b/ros/compros.sh deleted file mode 100755 index 8e40060..0000000 --- a/ros/compros.sh +++ /dev/null @@ -1,16 +0,0 @@ -#! /bin/bash - -export ROS_IP=192.168.1.11 -export ROS_MASTER_URI=http://192.168.1.3:11311 -export ROS_HOSTNAME=192.168.1.11 - -#source devel/setup.bash - -#echo "|$0| |$1|" - -#if [ "$1" == "mock" ] || [ "$1" == "Mock" ] -#then -# roslaunch launch/run_msurface.launch -#else -# roslaunch launch/run_surface.launch -#fi diff --git a/ros/launch/run_all.launch b/ros/launch/run_all.launch deleted file mode 100644 index 42ccc97..0000000 --- a/ros/launch/run_all.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros/launch/run_msurface.launch b/ros/launch/run_msurface.launch deleted file mode 100644 index f5bb06a..0000000 --- a/ros/launch/run_msurface.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/ros/launch/run_roslib.launch b/ros/launch/run_roslib.launch deleted file mode 100644 index 29da636..0000000 --- a/ros/launch/run_roslib.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/ros/launch/run_rov.launch b/ros/launch/run_rov.launch index 7083844..c3077c7 100644 --- a/ros/launch/run_rov.launch +++ b/ros/launch/run_rov.launch @@ -6,11 +6,10 @@ - - - + + + - diff --git a/ros/launch/run_rov_simple.launch b/ros/launch/run_rov_simple.launch deleted file mode 100644 index 48228b9..0000000 --- a/ros/launch/run_rov_simple.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/ros/launch/run_surface.launch b/ros/launch/run_surface.launch deleted file mode 100644 index 6e37d81..0000000 --- a/ros/launch/run_surface.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/ros/piros.sh b/ros/piros.sh index c71f8ee..b45bc29 100755 --- a/ros/piros.sh +++ b/ros/piros.sh @@ -1,6 +1,6 @@ #! /bin/bash -source ~/X13-Core/ros/devel/setup.bash +source ~/X14-Core/ros/devel/setup.bash export ROS_IP=192.168.1.3 export ROS_PYTHON_VERSION=3 -roslaunch ~/X13-Core/ros/launch/run_rov.launch +roslaunch ~/X14-Core/ros/launch/run_rov.launch diff --git a/ros/src/can_com/scripts/can_bus.py b/ros/src/can_com/scripts/can_bus.py index 6d1ae4a..56a8b8c 100755 --- a/ros/src/can_com/scripts/can_bus.py +++ b/ros/src/can_com/scripts/can_bus.py @@ -26,7 +26,7 @@ def topic_message_received(msg): data_list.append((msg.data >> shift) % 256) data = bytearray(data_list) rospy.loginfo('Topic Message Received: ' + str(msg.id) + ':' + str(list(data))) - can_tx = can.Message(arbitration_id=msg.id, data=data, extended_id=False) + can_tx = can.Message(arbitration_id=msg.id, data=data, is_extended_id=False) try: can_bus.send(can_tx, timeout=1) except can.CanError as cerr: diff --git a/ros/src/can_com/scripts/can_nuke.py b/ros/src/can_com/scripts/can_nuke.py index 7f0a9d8..047353d 100644 --- a/ros/src/can_com/scripts/can_nuke.py +++ b/ros/src/can_com/scripts/can_nuke.py @@ -7,19 +7,19 @@ boards = [0x201, 0x202, 0x203] while True: - packet = bytearray([140,140,140,140,140,140,140,140]) + packet = bytearray([140, 140, 140, 140, 140, 140, 140, 140]) for cid in boards: - can_tx = can.Message(arbitration_id=cid, data=packet, extended_id=False) + can_tx = can.Message(arbitration_id=cid, data=packet, extended_id=False) - can_bus.send(can_tx, timeout=0.001) + can_bus.send(can_tx, timeout=0.001) - print(cid, list(packet)) + print(cid, list(packet)) time.sleep(0.3) - packet = bytearray([140,140,140,140,127,127,127,127]) + packet = bytearray([140, 140, 140, 140, 127, 127, 127, 127]) for cid in boards: - can_tx = can.Message(arbitration_id=cid, data=packet, extended_id=False) + can_tx = can.Message(arbitration_id=cid, data=packet, extended_id=False) can_bus.send(can_tx, timeout=0.001) - print(cid, list(packet)) + print(cid, list(packet)) time.sleep(0.3) diff --git a/ros/src/can_com/scripts/chaotic_good.py b/ros/src/can_com/scripts/chaotic_good.py index 3f07de0..71b2275 100644 --- a/ros/src/can_com/scripts/chaotic_good.py +++ b/ros/src/can_com/scripts/chaotic_good.py @@ -4,6 +4,7 @@ import time IDS = [513, 514, 515] +N_CAN_BYTES = 8 POS_RANGE = 4 DEFAULT_POWER = 146 ZERO_POWER = 127 @@ -19,15 +20,15 @@ channel = sys.argv[1] can_bus = can.interface.Bus(channel=channel, bustype='socketcan') - for i in range(8): - data_list = list() + for i in range(N_CAN_BYTES): + data_list = [] [data_list.append(0) for _ in range(i)] data_list.append(DEFAULT_POWER) - [data_list.append(0) for _ in range(i + 1, 8)] + [data_list.append(0) for _ in range(i + 1, N_CAN_BYTES)] # [data_list.append(DEFAULT_POWER) for _ in range(8)] data = bytearray(data_list) - print str(I) + ' : ' + str(list(data)) + print(f"{I} : {list(data)}") can_tx = can.Message(arbitration_id=I, data=data, extended_id=False) can_bus.send(can_tx) diff --git a/ros/src/can_com/scripts/solenoid_test.py b/ros/src/can_com/scripts/solenoid_test.py deleted file mode 100644 index 4bc4e14..0000000 --- a/ros/src/can_com/scripts/solenoid_test.py +++ /dev/null @@ -1,37 +0,0 @@ -#! /usr/bin/python -import sys -import can -import time - -IDS = [516] -POS_RANGE = 4 -DEFAULT_POWER = 200 -ZERO_POWER = 127 -DELAY = 1 - - -# This is a test script intended to simplify identification of hardware thruster -# configuration by sending commands to each possible thruster position one at a time. - - -if __name__ == "__main__": - channel = 'can0' - if len(sys.argv) == 2: - channel = sys.argv[1] - can_bus = can.interface.Bus(channel=channel, bustype='socketcan') - - values = [0x80,0x40,0x20,0x10,0x08,0x04,0x02,0x01,0x00] - # values = [0x20,0x08] - for _ in range(100000000000): - for v in values: - data_array = [0] * 8; - for i in range(0,8): - data_array[i]=v - #data_array[-1] = v - data = bytearray(data_array) - - print("SOL: %s" % bin(v)) - can_tx = can.Message(arbitration_id=516, data=data, extended_id=False) - can_bus.send(can_tx, timeout=0.1) - - time.sleep(DELAY) diff --git a/ros/src/can_com/scripts/thrust_config.py b/ros/src/can_com/scripts/thrust_config.py index 219d997..e7b0883 100644 --- a/ros/src/can_com/scripts/thrust_config.py +++ b/ros/src/can_com/scripts/thrust_config.py @@ -37,9 +37,9 @@ data_list.append(ZERO_POWER - POWER_DELTA) # adds DEFAULT POWER at target pos [data_list.append(ZERO_POWER) for _ in range(POS_RANGE - p - 1)] # adds ZERO POWER buffers after target pos data = bytearray(data_list) - print str(i) + ' : ' + str(list(data)) + print(str(i) + ' : ' + str(list(data))) - can_tx = can.Message(arbitration_id=i, data=data, extended_id=False) + can_tx = can.Message(arbitration_id=i, data=data, is_extended_id=False) can_bus.send(can_tx) time.sleep(DELAY) @@ -49,8 +49,8 @@ [data_list.append(0) for _ in range(4)] [data_list.append(ZERO_POWER) for _ in range(4, 8)] data = bytearray(data_list) - print str(i) + ' : ' + str(list(data)) + ' STOP' - can_tx = can.Message(arbitration_id=i, data=data, extended_id=False) + print(str(i) + ' : ' + str(list(data)) + ' STOP') + can_tx = can.Message(arbitration_id=i, data=data, is_extended_id=False) can_bus.send(can_tx) time.sleep(DELAY) @@ -73,9 +73,9 @@ [data_list.append(0) for _ in range(4)] [data_list.append(ZERO_POWER) for _ in range(4, 8)] data = bytearray(data_list) - print str(i) + ' : ' + str(list(data)) + ' STOP' + print( str(i) + ' : ' + str(list(data)) + ' STOP') - can_tx = can.Message(arbitration_id=i, data=data, extended_id=False) + can_tx = can.Message(arbitration_id=i, data=data, is_extended_id=False) can_bus.send(can_tx) time.sleep(DELAY) diff --git a/ros/src/can_proc/scripts/.thrust_proc.py.swp b/ros/src/can_proc/scripts/.thrust_proc.py.swp deleted file mode 100644 index ca9f714..0000000 Binary files a/ros/src/can_proc/scripts/.thrust_proc.py.swp and /dev/null differ diff --git a/ros/src/can_proc/scripts/.tool_proc.py.swp b/ros/src/can_proc/scripts/.tool_proc.py.swp deleted file mode 100644 index 0b66a74..0000000 Binary files a/ros/src/can_proc/scripts/.tool_proc.py.swp and /dev/null differ diff --git a/ros/src/can_proc/scripts/2021-04-16-04-42-06.bag b/ros/src/can_proc/scripts/2021-04-16-04-42-06.bag new file mode 100644 index 0000000..ce8e64d Binary files /dev/null and b/ros/src/can_proc/scripts/2021-04-16-04-42-06.bag differ diff --git a/ros/src/can_proc/scripts/thrust_proc.py b/ros/src/can_proc/scripts/thrust_proc.py index be8bd3c..41291e2 100755 --- a/ros/src/can_proc/scripts/thrust_proc.py +++ b/ros/src/can_proc/scripts/thrust_proc.py @@ -10,8 +10,8 @@ can_pos = [5, 6, 7, 5, 6, 4, 5, 7] # positions in data packet can_better_map = { - 0x201: [ 3, 7, 2, 6 ], - 0x202: [ 8, 4, 5, 1 ], + 0x201: [ 3, 7, 1, 5 ], + 0x202: [ 8, 4, 6, 2 ], 0x203: [ 0, 0, 0, 0 ] } diff --git a/ros/src/can_proc/scripts/tool_proc.py b/ros/src/can_proc/scripts/tool_proc.py index f1cefc7..cf9822d 100755 --- a/ros/src/can_proc/scripts/tool_proc.py +++ b/ros/src/can_proc/scripts/tool_proc.py @@ -1,16 +1,38 @@ #! /usr/bin/python3 import rospy from shared_msgs.msg import can_msg, tools_command_msg +from gpiozero import OutputDevice + +pm = OutputDevice(12) TOOLS_BOARD_ID = 0x204 #PM_BIT = 0b00001000 #GHOST_BIT = 0b11110111 +BLUE = 1 << 1 +BROWN = 1 << 2 +YELLOW = 1 << 3 +GREEN = 1 << 4 +WHITE = 1 << 5 + +#============================= +#Grant told me to put it here, don't delete yet +#yeet - green +#am - brown +#pm - blue +#bs - yello +#blue - 0b00000010 +#brown - 0b00000100 +#yellow - 0b00001000 +#green - 0b00010000 +#white - 0b00100000 +#============================= + #Easier swapping if we plug solenoids in differently sol0 = 1 << 1 sol1 = 1 << 3 -sol2 = 1 << 5 +sol2 = 1 << 4 SECONDARY_BIT = sol0 PM_BIT = sol1 @@ -24,24 +46,19 @@ def message_received(msg): - cmd = (msg.pm * PM_BIT) - cmd |= (msg.ghost * GHOST_BIT) - cmd |= (msg.secondary * SECONDARY_BIT) - - cmsg = can_msg() - cmsg.id = TOOLS_BOARD_ID - cmsg.data = cmd - pub.publish(cmsg) + if(msg.tools[0]): + pm.on() + else: + pm.off() if __name__ == "__main__": rospy.init_node('tool_proc', anonymous=True) - # Publish to the CAN hardware transmitter pub = rospy.Publisher('can_tx', can_msg, queue_size=10) - sub = rospy.Subscriber('/tools_proc', tools_command_msg, + sub = rospy.Subscriber('/tools', tools_command_msg, message_received) # rate = rospy.Rate(5) # 5hz diff --git a/ros/src/control/scripts/Complex_1.pyc b/ros/src/control/scripts/Complex_1.pyc deleted file mode 100644 index 8edad9c..0000000 Binary files a/ros/src/control/scripts/Complex_1.pyc and /dev/null differ diff --git a/ros/src/control/scripts/Complex_2.py b/ros/src/control/scripts/Complex_2.py deleted file mode 100644 index f449d11..0000000 --- a/ros/src/control/scripts/Complex_2.py +++ /dev/null @@ -1,296 +0,0 @@ -from numpy import linalg -import numpy as np -import pprint as pp -import init_hw_constants - - -class Complex_2(): - """ - This code is a rewrite of MutatorMatrix to simplify and clarify the code without removing the source - This thrust-mapping works by solving the least squared solution of a system determined by the thrusters locations - and the directions of the thrusters. - The system is the 6x8 matrix made of columns of each thrusters affect on each component X, Y, Z. ROLL, PITCH, YAW - multiplied by the 8x1 thrust map matrix (which we are solving for) equal to the 6x1 desired thrust matrix - To find the least square solution we find the pseudo-inverse of the 6x8 matrix (A) and multiply both sides of the - equation by it. If a inverse of the matrix A exists the pseudo-inverse(A) = inverse(A) if not then - pseudo-inverse(A) * A can be ignored because math leaving - thrust map matrix = pseudo-inverse(A) * desired thrust - For location and rotation vectors, the first four thrusters are the horizontal thrusters: first is front left, - second is front right, third is back left, and fourth is back right. The last four are the vertical thrusters in - the same order: front left, front right, back left, and back right. - Outside classes use this class to find the pwm values for each thruster based on force input. The _calculate - function returns the 8D pwm vector for the thrusters, and the _get_results function returns the force vector based - on the pwm vector. The thrust and power output of each thruster are in the arrays thrust and power, and the total - power can be accessed with the variable final_power. - This code is different from Complex_1 based on solely the number of thrusters and is being used to test the results - of using more than 8 thrusters. - """ - # X11 Thruster locations and center of mass relative to an arbitrary(?) point converted from inches to meters - # Each column is X, Y, Z: X is forward/back, Y is left/right, Z is up/down - X11_THRUSTERS = np.matrix([ - [6.7593, 6.7593, -6.7593, -6.7593, 7.6887, 7.6887, 7.6887, -7.6887, -7.6887, -7.6887], - [-6.625, 6.625, -6.625, 6.625, -3.75, 0, 3.75, -3.75, 0, 3.75], - [-0.5809, -0.5809, -0.5809, -0.5809, 4.8840, 4.8840, 4.8840, 4.8840, 4.8840, 4.8840] - ]) * 0.0254 - - X = 1.8 - Y = 0 - Z = 0 - - X11_COM = np.matrix([ - [X, X, X, X, X, X, X, X, X, X], - [Y, Y, Y, Y, Y, Y, Y, Y, Y, Y], - [Z, Z, Z, Z, Z, Z, Z, Z, Z, Z] - ]) * 0.0254 - - # X and Y component of horizontal thrusters converted to radians to be used by numpy 7pi/18 rad = 70 degrees - X_COMPONENT = np.sin(7 * np.pi / 18) - Y_COMPONENT = np.cos(7 * np.pi / 18) - - ROTATION = np.matrix([ - [X_COMPONENT, X_COMPONENT, -X_COMPONENT, -X_COMPONENT, 0, 0, 0, 0, 0, 0], - [Y_COMPONENT, -Y_COMPONENT, Y_COMPONENT, -Y_COMPONENT, 0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 1, 1, 1, 1, 1, 1] - ]) - - def __init__(self): - self.thruster_layout = np.matrix(Complex_2.X11_THRUSTERS - Complex_2.X11_COM) - # this is the 6x8 matrix that specifies each thrusters contribution on X, Y, Z, Roll, Pitch, Yaw - self.matrix = None - # the pseudo inverse of self.matrix used to find the least square solution - self.pseudo_inverse_matrix = None - # list of disabled thrusters used to determine when matrix and pseudo_inverse_matrix need to be updated - len_list = Complex_2.X11_THRUSTERS.shape[1] - self.disabled = [False for col in range(len_list)] - # The last thrust map returned by the calculate function - self.map = None - - # initialize thrust and power vectors, and total power value - self.thrust = np.matrix(np.zeros(len_list)) - self.power = np.matrix(np.zeros(len_list)) - self.final_power = 0.0 - - self._generate_matrix() - - def calculate(self, desired_thrust, disabled_thrusters=None, disable_limiting=False): - """ - Calculate the needed thrust for each thruster to achieve desired - :param desired_thrust: The 6 dimensional vector which we want to achieve vector as 6x1 matrix - :param disabled_thrusters: list of 8 items each corresponding to a thruster (0 meaning enabled, 1 disabled) - :return: the thrust map generated by solving for the least square solution of the equation - """ - # In the case of a thruster malfunction this allows for the pseudo_inverse_matrix to be recalculated - # to account for the thruster that no longer works - if disabled_thrusters != self.disabled: - self.disabled = disabled_thrusters - self._generate_matrix() - - # print desired_thrust - - self.map = self.pseudo_inverse_matrix.dot(desired_thrust) - - self._normalize(desired_thrust) - initial_power, limitPower = self._calc_thrust_power(self.map) - # limit power if necessary: - self.final_power = initial_power - iteration = 0 - while limitPower == 1 and disable_limiting == False: - if iteration > 3: - print('Limit power function iteration limit exceeded, assume values are close enough.') - break - self.final_power = self._limit_power(initial_power) - self.final_power, limitPower = self._calc_thrust_power(self.map) - print('Power was limited, force vector changed!') - return self.map.tolist()[0] - - def _generate_matrix(self): - """ - Generate the pseudo-inverse of the matrix to be used in the calculation - :return: the pseud-inverse of self.matrix - """ - # Calculate the cross product between the location of each thruster and the direction it points in - rot = np.transpose(np.cross(np.transpose(self.ROTATION), np.transpose(self.thruster_layout), 1)) - self.matrix = np.concatenate((self.ROTATION, rot)) - for thruster in range(len(self.disabled)): - if self.disabled[thruster]: - self.matrix[:, thruster] = 0.0 - self.pseudo_inverse_matrix = linalg.pinv(self.matrix) - return self.pseudo_inverse_matrix - - def _normalize(self, desired_thrust): - """ - Normalize the values of the thrust map to be in the range [-max_force, max_force] if necessary - :return: None - """ - max_val = np.amax(np.abs(self.map)) - if max_val == 0: - max_val = 1 - - max_force = np.amax(np.abs(desired_thrust)) - - self.map *= (max_force / max_val) - - def _limit_power(self, initialPower): - """ - Ensure power limit is not exceeded by scaling the thruster values down if necessary - :return: limitedPower - """ - limitedPower = 0.0 - # initialize maxPower as lowest power value - maxPower = 0.51 - maxPowerIndex = 0 - num_thrusters = len(self.disabled) - for thruster in range(num_thrusters): - if self.power[0, thruster] > maxPower: - maxPower = self.power[0, thruster] - maxThrust = self.thrust[0, thruster] - maxPowerIndex = thruster - orig_thrust_maxP = maxThrust - self.thrust[0, maxPowerIndex] = self._power_to_thrust(init_hw_constants.POWER_THRESH, orig_thrust_maxP) - overMaxPower = np.matrix(np.zeros(num_thrusters)) - # find thrusters with over power threshold and make them the threshold value based on PWM value and - # mark which were changed - for thruster in range(num_thrusters): - if self.thrust[0, thruster] < 0: - while self._pwm_to_power(self.map[0, thruster]) > init_hw_constants.POWER_THRESH: - self.map[0, thruster] = self.map[0, thruster] + 0.005 - overMaxPower[0, thruster] = 1 - if self.thrust[0, thruster] > 0: - while self._pwm_to_power(self.map[0, thruster]) > init_hw_constants.POWER_THRESH: - self.map[0, thruster] = self.map[0, thruster] - 0.005 - overMaxPower[0, thruster] = 1 - if thruster == maxPowerIndex: - self.thrust[0, maxPowerIndex] = self._power_to_thrust(self._pwm_to_power(self.map[0, thruster]), - orig_thrust_maxP) - # change thrust values to - for thruster in range(num_thrusters): - if thruster != maxPowerIndex: - self.thrust[0, thruster] = self.thrust[0, thruster] * self.thrust[0, maxPowerIndex] / orig_thrust_maxP - self.power[0, thruster] = self._thrust_to_power(self.thrust[0, thruster]) - limitedPower = limitedPower + self.power[0, thruster] - if overMaxPower[0, thruster] != 1: - self.map[0, thruster] = self._thrust_to_pwm(self.thrust[0, thruster]) - return limitedPower - - def _calc_thrust_power(self, thrusters): - """ - Find the total power used by all 8 thrusters for given pwm values - Also calculate the thrust and power for each individual thruster for global variables - :return: totalPower, limitPower - """ - # calculate thrust output and power used values for each thruster - totalPower = 0.0 - limitPower = 0 - num_thrusters = len(self.disabled) - for thruster in range(num_thrusters): - pwm_output = thrusters[0, thruster] - self.thrust[0, thruster] = self._pwm_to_thrust(pwm_output) - self.power[0, thruster] = self._pwm_to_power(pwm_output) - totalPower = totalPower + self.power[0, thruster] - # set flag to limit power if any use more than allocated threshold value (based on power design) - if self.power[0, thruster] > init_hw_constants.POWER_THRESH: - limitPower = 1 - return totalPower, limitPower - - def _pwm_to_thrust(self, pwm): - """ - Change PWM value to thrust value based on 12V data from thrusters - :return: Thrust Value (lbf) - """ - if pwm < -0.05: - thrustVal = -3.6529 * (pwm ** 3) - 9.8279 * (pwm ** 2) + 0.5183 * pwm - 0.04 - elif pwm > 0.05: - thrustVal = -5.9996 * (pwm ** 3) + 13.296 * (pwm ** 2) + 0.4349 * pwm + 0.0345 - else: - thrustVal = 0 - return thrustVal - - def _pwm_to_power(self, pwm): - """ - Convert PWM value to power value based on 12V data from thrusters - :return: Power Value (W) - """ - if pwm < 0: - powerVal = -53.282 * (pwm ** 3) + 135.58 * (pwm ** 2) + 1.1986 * pwm + 0.51 - else: - powerVal = 35.949 * (pwm ** 3) + 150.51 * (pwm ** 2) - 3.0096 * pwm + 0.51 - return powerVal - - def _power_to_thrust(self, powerVal, sign): - """ - Convert power value to thrust value based on 12V data from thrusters, given desired sign of thrust - Note that sign can be any positive or negative value, or 0 if thrust is zero - :return: thrust value (lbf) - """ - if sign > 0: - thrustVal = 0.000001 * (powerVal ** 3) - 0.0004 * (powerVal ** 2) + 0.0855 * powerVal - elif sign < 0: - thrustVal = -0.0000008 * (powerVal ** 3) + 0.0003 * (powerVal ** 2) - 0.0697 * powerVal - else: - thrustVal = 0 - return thrustVal - - def _thrust_to_power(self, thrustVal): - """ - Convert thrust value to power value based on 12V data from thrusters - :return: power value (W) - """ - if thrustVal < 0: - powerVal = 2.3329 * (thrustVal ** 2) - 12.016 * thrustVal + 0.0959 - if thrustVal > 0: - powerVal = 1.8977 * (thrustVal ** 2) + 8.37 * thrustVal + 1.2563 - else: - powerVal = 0.51 - return powerVal - - def _thrust_to_pwm(self, thrustVal): - """ - Convert thrust value to PWM value based on 12V data from thrusters - :return: PWM value - """ - if thrustVal < 0: - pwm = 0.0021 * (thrustVal ** 3) + 0.0298 * (thrustVal ** 2) + 0.2426 * thrustVal - 0.0775 - elif thrustVal > 0: - pwm = 0.0017 * (thrustVal ** 3) - 0.025 * (thrustVal ** 2) + 0.213 * thrustVal + 0.0675 - else: - # assume 0 even though dead band has range of pwm values - pwm = 0 - return pwm - - def _get_results(self): - """ - Method for returning what the resulting thrust map would cause for the rov based - on the thruster locations and angles. - N.B. the normalize function can cause these values to be radically different than the intended. Disable it - in the calculate function to get more accurate results. If normalized it should be correct just scaled down - :return: dict of X, Y, Z, Roll, Pitch, Yaw - """ - res = c.matrix * np.transpose(c.map) - result = { - 'X': res[0, 0], - 'Y': res[1, 0], - 'Z': res[2, 0], - 'Roll': res[3, 0], - 'Pitch': res[4, 0], - 'Yaw': res[5, 0] - } - return result - - -if __name__ == '__main__': - c = Complex_2() - np.set_printoptions(linewidth=150, suppress=True) - print('MATRIX') - pp.pprint(c.matrix) - print('\nPSEUDO-INVERSE MATRIX') - pp.pprint(c.pseudo_inverse_matrix) - print('\nRESULT 8D VECTOR') - pp.pprint(c.calculate(np.array([0, 0, 1, 0, 0, 0]), [0, 0, 0, 0, 0, 0, 0, 0, 0, 0], False)) - print('\nTHRUST') - pp.pprint(c.thrust) - print('POWER') - pp.pprint(c.power) - print('\nTOTAL POWER') - pp.pprint(c.final_power) - print('\nRESULTING 6D VECTOR') - pp.pprint(c._get_results()) diff --git a/ros/src/control/scripts/PI_Controller.py b/ros/src/control/scripts/PI_Controller.py deleted file mode 100644 index 5f2f825..0000000 --- a/ros/src/control/scripts/PI_Controller.py +++ /dev/null @@ -1,38 +0,0 @@ -import numpy - - -class PI(object): - """A generic PID loop controller which can be inherited and used in other control algorithms""" - - def __init__(self, p=1, i=0): - """Return a instance of a un tuned PID controller""" - self._p = p - self._i = i - self._esum = 0 # Error sum for integral term - self._count = 0 - - def calculate(self, error, dt): - """Calculates the output of the PI controller""" - self._esum += error * dt - u = self._p * error + self._i * self._esum - return u - - def reset(self): - """Resets the integral sum""" - self._esum = 0 - - @property - def p(self): - return self._p - - @p.setter - def p(self, value): - self._p = value - - @property - def i(self): - return self._i - - @i.setter - def i(self, value): - self._i = value diff --git a/ros/src/control/scripts/Position_Controller.py b/ros/src/control/scripts/Position_Controller.py deleted file mode 100644 index dfe0cd8..0000000 --- a/ros/src/control/scripts/Position_Controller.py +++ /dev/null @@ -1,31 +0,0 @@ -from PI_Controller import PI - - -class Position_Controller(object): - - def __init__(self, LOOP_CONSTANTS): - self._isActive = False - self._PIloop = PI(LOOP_CONSTANTS[0], LOOP_CONSTANTS[1]) - self._setpoint = 0.0 - - def activate(self): - self._isActive = True - - def deactivate(self): - self._isActive = False - - def toggle(self): - self._isActive = not self._isActive - - def set_setpoint(self, setpoint): - # need to add data verification such as checking bounds of setpoint - self._setpoint = setpoint - - def calculate(self, setpoint, sensor_data, dt): - if (not self._isActive): - return 0.0 - error = setpoint - sensor_data - return PI.calculate(error, dt) - - def calculate(self, sensor_data, dt): - return calculate(setpoint, sensor_data, dt) diff --git a/ros/src/control/scripts/ROV_main.py b/ros/src/control/scripts/ROV_main.py index 9c02ea0..e98a42b 100755 --- a/ros/src/control/scripts/ROV_main.py +++ b/ros/src/control/scripts/ROV_main.py @@ -1,7 +1,7 @@ #! /usr/bin/python3 import rospy import enum -from shared_msgs.msg import controller_msg, thrust_command_msg, thrust_disable_inverted_msg, tools_command_msg, rov_velocity_command +from shared_msgs.msg import controller_msg, thrust_command_msg, thrust_disable_inverted_msg, rov_velocity_command from std_msgs.msg import Bool from geometry_msgs.msg import Twist class Coord(enum.Enum): @@ -15,68 +15,32 @@ class Contr_Type(enum.Enum): controller_tools_command = [0,0,0,0] translation_Scaling = 3.2 rotation_Scaling = 1.5 +mode_fine = True +fine_multiplier = 1.041 def onLoop(): #Thruster Control thrust_command = thrust_command_msg() thrust_command.desired_thrust = controller_percent_power + thrust_command.isFine = mode_fine + thrust_command.multiplier = fine_multiplier thrust_command_pub.publish(thrust_command) - #Tools Control - tools_command = tools_command_msg() - tools_command.pm = controller_tools_command[0] - tools_command.ghost = controller_tools_command[1] - tools_command.secondary = controller_tools_command[2] - tools_command_pub.publish(tools_command) def _velocity_input(msg): + global mode_fine, fine_multiplier controller_percent_power[0] = msg.twist.linear.x controller_percent_power[1] = msg.twist.linear.y controller_percent_power[2] = msg.twist.linear.z controller_percent_power[3] = msg.twist.angular.x controller_percent_power[4] = msg.twist.angular.y controller_percent_power[5] = msg.twist.angular.z - -def _controller_input(contr): - controller_percent_power[0] = contr.LY_axis * translation_Scaling # translational - controller_percent_power[1] = contr.LX_axis * translation_Scaling * .5 # translation - controller_percent_power[2] = ((contr.Rtrigger) - (contr.Ltrigger)) * translation_Scaling - if contr.a == 1: - controller_percent_power[3] = 1 * rotation_Scaling - elif contr.b == 1: - controller_percent_power[3] = -1 * rotation_Scaling - else: - controller_percent_power[3] = 0.0 - controller_percent_power[4] = -contr.RY_axis * rotation_Scaling * .5# pitch - controller_percent_power[5] = contr.RX_axis * rotation_Scaling # yaw - if contr.x == 1: - controller_tools_command[0] = 1 - if contr.y ==1: - controller_tools_command[0] = 0 -def _pm_set_state(msg): - if(msg.data): - controller_tools_command[0] = 1 - else: - controller_tools_command[0] = 0 -def _gh_set_state(msg): - if(msg.data): - controller_tools_command[1] = 1 - else: - controller_tools_command[1] = 0 -def _bs_set_state(msg): - if(msg.data): - controller_tools_command[2] = 1 - else: - controller_tools_command[2] = 0 + mode_fine = msg.isFine + fine_multiplier = msg.multiplier if __name__ == '__main__': rospy.init_node('ROV_main') velocity_sub = rospy.Subscriber('/rov_velocity', rov_velocity_command,_velocity_input) - pm_sub = rospy.Subscriber('/pm_cmd',Bool, _pm_set_state) - gh_sub = rospy.Subscriber('/gh_cmd',Bool, _gh_set_state) - bs_sub = rospy.Subscriber('/bs_cmd',Bool, _bs_set_state) - #controller_sub = rospy.Subscriber('/gamepad_listener', controller_msg,_controller_input) thrust_command_pub = rospy.Publisher('/thrust_command', thrust_command_msg, queue_size=1) - tools_command_pub = rospy.Publisher('/tools_proc', tools_command_msg, queue_size=10) r = rospy.Rate(50) while not rospy.is_shutdown(): onLoop() diff --git a/ros/src/control/scripts/Simple_1.py b/ros/src/control/scripts/Simple_1.py deleted file mode 100644 index df55008..0000000 --- a/ros/src/control/scripts/Simple_1.py +++ /dev/null @@ -1,109 +0,0 @@ -""" - Software's make-shift thrustmapping to try and circumvent some of the nuances of the - Complex thrustmapping's implementation that exist without proper pool time to debug them - out of the system - - [X, Y, Z, Roll, Pitch, Yaw] assumed -""" - -import numpy as np -from pprint import pprint as pp - -## The indexis of the thrusters -## Change this to configure the output -mappings = { - "HFL": 0, - "HFR": 1, - "HBR": 2, - "HBL": 3, - "VFL": 4, - "VFR": 5, - "VBR": 6, - "VBL": 7, -} - -## The weights of each direction's influence -## Tweak these to scale the directional movement -weights = { - "X": .40, - "Y": .40, - "Z": .30, - "Roll": .125, - "Pitch": .40, - "Yaw": .20 -} - -## The thrusters associated with each direction, and their polarization -## (front/back for positive direction value) -## Change these to influence what thrusters get influenced how by each direction -maskScaffolding = { - "X": {"HFR": 1, "HFL": 1, "HBL": -1, "HBR": -1}, - "Y": {"HFR": 1, "HFL": -1, "HBL": -1, "HBR": 1}, - "Z": {"VFR": 1, "VFL": 1, "VBL": 1, "VBR": 1}, - "Roll": {"VFR": -1, "VFL": 1, "VBL": 1, "VBR": -1}, - "Pitch": {"VFR": -1, "VFL": -1, "VBL": 1, "VBR": 1}, - "Yaw": {"HFR": 1, "HFL": -1} -} - - -## This method returns a numpy array of the masks, in the order given from top down -## For the default order, top row is X's influences, second row is Y's influences, etc. -def getMasks(order=["X", "Y", "Z", "Roll", "Pitch", "Yaw"]): - res = [] - - for key in order: - temp = [0] * 8 - - for thrust in maskScaffolding[key]: - temp[mappings[thrust]] = weights[key] * maskScaffolding[key][thrust] - # print("temp[mappings[{}]] = weights[{}] * maskScaffolding[{}][{}]".format(thrust, key, key, thrust)) - - res.append(temp) - # print("{}:".format(key)) - # print(res[key]) - - return np.asarray(res) - - -## Scales down the whole array to be under the given percentage (default, 80% thrust) -def scaleDown(arry, thresh=0.8): - biggest = np.amax(arry) - - if biggest > thresh: - pp("Scaled down!") - pp(arry) - scale = thresh / biggest - arry = arry * scale - pp(arry) - - return arry - - -## Runs the simple calculation to check what the combined output should be -def calculateSimple(desired, order=["X", "Y", "Z", "Roll", "Pitch", "Yaw"]): - vecDict = {} - vecMask = {} - - masks = getMasks(order=order) - - for i in range(len(desired)): - key = order[i] - vecDict[key] = desired[i] - - # print(key) - # print(masks[i]) - - ## Get the 8-vector controls from the masks against the 6-dof desired movement - res = scaleDown(np.matmul(masks.transpose(), np.asarray(desired)), thresh=0.8) - # print(res) - - return res - - -if __name__ == "__main__": - for i in range(6): - for j in range(6): - a = [0] * 6 - a[i] = 1 - a[j] = 1 - calculateSimple(a) diff --git a/ros/src/control/scripts/auto_control.py b/ros/src/control/scripts/auto_control.py deleted file mode 100755 index 47d727a..0000000 --- a/ros/src/control/scripts/auto_control.py +++ /dev/null @@ -1,96 +0,0 @@ -#! /usr/bin/python -import rospy -from shared_msgs.msg import can_msg, auto_control_msg, auto_command_msg - -# TODO: UNCOMMENT BELOW WHEN THRUST_CONTROL BRANCH MERGED WITH I2C BRANCH -# from shared_msgs.msg import imu_msg - -from std_msgs.msg import Float32 - -setpoints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -imu_output = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -thrust_data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - -# TODO: UNCOMMENT BELOW WHEN THRUST_CONTROL BRANCH MERGED WITH I2C BRANCH -# prev_time = 0 -# locked = [False, False, False, False, False, False] -# initialize PID loops for each rotation vector with values for P and I of the PID -# pid_roll = pid_file.Position_Controller(P, I, roll) -# pid_pitch = pid_file.Position_Controller(P, I, pitch) -# pid_yaw = pid_file.Position_Controller(P, I, yaw) - -def imu_received(msg): - global imu_output - # global prev_time - # global locked - # global pid_roll - # global pid_pitch - # global pid_yaw - - # TODO: UNCOMMENT BELOW WHEN THRUST_CONTROL BRANCH MERGED WITH I2C BRANCH - # imu_output = [msg.gyro, msg.accel] - - # TODO: send data and setpoints to pid object - # thrust_roll = 0.0 - # thrust_pitch = 0.0 - # thrust_yaw = 0.0 - # time = rospy.get_rostime().secs - prev_time - # if locked[3]: - # thrust_roll = pid_roll.calculate(setpoints[3], imu_output, rospy.get_rostime().secs) - # if locked[4]: - # thrust_pitch = pid_pitch.calculate(setpoints[4], imu_output, rospy.get_rostime().secs) - # if locked[5]: - # thrust_yaw = pid_yaw.calculate(setpoints[5], imu_output, rospy.get_rostime().secs) - # thrust_data = [0, 0, 0, thrust_roll, thrust_pitch, thrust_yaw] - # output_msg = auto_control_msg() - # output_msg.thrust_vec = thrust_data - # output_msg.dims_locked = [False, False, False, False, False, False] - # pub.publish(output_msg) - - -def depth_received(msg): - # do nothing for now; only focusing on roll, pitch, and yaw first - pass - - -def command_received(msg): - # global locked - # for i in range(0, 6): - # if msg.stabilization_dim[i] == 1: - # locked[i] = True - # else: - # locked[i] = False - sample_message = auto_control_msg() - sample_message.thrust_vec = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - sample_message.dims_locked = [False, False, False, False, False, False] - pub.publish(sample_message) - - -if __name__ == "__main__": - rospy.init_node('auto_control') - - # receive data - # TODO: UNCOMMENT THIS; COMMENTED BECAUSE NOT USING STANDARD IMU MSG FILE SO Imu WILL CAUSE AN ERROR - # imu_sub = rospy.Subscriber('imu', Imu, - # imu_received) - depth_sub = rospy.Subscriber('depth', Float32, - depth_received) - command_sub = rospy.Subscriber('/surface/auto_command', - auto_command_msg, command_received) - - # send data - pub = rospy.Publisher('auto_control', - auto_control_msg, queue_size=10) - - # TODO: initialize the pid object - - rate = rospy.Rate(10) # 10hz - - while not rospy.is_shutdown(): - sample_message = auto_control_msg() - sample_message.thrust_vec = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - sample_message.dims_locked = [False, False, False, False, False, False] - # TODO: set sample_message to thrust_data - pub.publish(sample_message) - rate.sleep() diff --git a/ros/src/control/scripts/dumbController.py b/ros/src/control/scripts/dumbController.py deleted file mode 100755 index 0791d13..0000000 --- a/ros/src/control/scripts/dumbController.py +++ /dev/null @@ -1,28 +0,0 @@ -#! /usr/bin/python -import rospy -import time -from shared_msgs.msg import controller_msg - -if __name__ == "__main__": - rospy.init_node("dumb_controller") - pub = rospy.Publisher("/surface/controller", controller_msg, queue_size=2) - while (True): - time.sleep(.1) - m = controller_msg() - - x = float(input("give and X value:")) - y = float(input("give and Y value:")) - z = float(input("give and Z value:")) - t = float(input("give and Throttle value:")) - rt = float(input("give a Right trigger value")) - lt = float(input("give a Left trigger value")) - m.RX_axis = x - m.RY_axis = y - m.LX_axis = z - m.LY_axis = t - m.Rtrigger = rt - m.Ltrigger = lt - pub.publish(m) - print(m) - print("-----------published------------") - print("-----press ctrl-z to exit-------") diff --git a/ros/src/control/scripts/init_hw_constants.pyc b/ros/src/control/scripts/init_hw_constants.pyc deleted file mode 100644 index 6f6ee85..0000000 Binary files a/ros/src/control/scripts/init_hw_constants.pyc and /dev/null differ diff --git a/ros/src/control/scripts/thrust_control.py b/ros/src/control/scripts/thrust_control.py index 6316687..c2f5304 100755 --- a/ros/src/control/scripts/thrust_control.py +++ b/ros/src/control/scripts/thrust_control.py @@ -1,11 +1,11 @@ #! /usr/bin/python3 import rospy -from shared_msgs.msg import auto_control_msg, final_thrust_msg, thrust_status_msg, thrust_command_msg, controller_msg +from shared_msgs.msg import auto_control_msg, final_thrust_msg, thrust_status_msg, thrust_command_msg, controller_msg, com_msg from sensor_msgs.msg import Imu from std_msgs.msg import Float32, String import numpy as np import Complex_1 -import thrust_mapping +from thrust_mapping import ThrustMapper import json from dynamic_reconfigure.server import Server from control.cfg import ROV_COMConfig @@ -16,7 +16,11 @@ locked_dims_list = [False, False, False, False, False, False] disabled_list = [False, False, False, False, False, False, False, False] inverted_list = [0, 0, 0, 0, 0, 0, 0, 0] -MAX_CHANGE = .1 +desired_thrust_final = [0, 0, 0, 0, 0, 0] +MAX_CHANGE = .15 +mode_fine = True +fine_multiplier = 1.041 +tm = ThrustMapper() # watch dog stuff last_packet_time = 0.0 is_timed_out = False @@ -29,10 +33,14 @@ def _pilot_command(comm): global disabled_list # disabled thrusters global inverted_list # inverted thrusters global desired_p_unramped + global tm #print (comm.desired_thrust) desired_p = comm.desired_thrust + tm.set_multiplier(comm.multiplier) + tm.set_fine(comm.isFine) # disabled_list = comm.disable_thrusters # inverted_list = comm.inverted + on_loop() def ramp(index): @@ -61,7 +69,7 @@ def on_loop(): # calculate thrust #pwm_values = c.calculate(desired_thrust_final, disabled_list, False) - desired_p_unramped = tm.thrustVectorToPWM(tm.calculateThrusterOutput(desired_thrust_final)) + desired_p_unramped = [tm.thrust_to_pwm(val) for val in tm.thruster_output(desired_p)] # invert relevant values #for i in range(8): # if inverted_list[i] == 1: @@ -69,7 +77,7 @@ def on_loop(): for i in range(0,8): ramp(i) pwm_values = desired_thrusters - #print(desired_p_unramped) + # assign values to publisher messages for thurst control and status tcm = final_thrust_msg() # val = float of range(-1, 1) @@ -79,24 +87,21 @@ def on_loop(): for i in range(0,8): thrusters[i] = int((pwm_values[i] + 1) * 127.5) tcm.thrusters = thrusters - # tcm.thrusters[0] = int((pwm_values[0] + 1) * 127.5) - # tcm.thrusters[0] = int((pwm_values[1] + 1) * 127.5) - # tcm.thrusters[0] = int((pwm_values[2] + 1) * 127.5) - # tcm.thrusters[0] = int((pwm_values[3] + 1) * 127.5) - # tcm.vfl = int((pwm_values[4] + 1) * 127.5) - # tcm.vfr = int((pwm_values[5] + 1) * 127.5) - # tcm.vbr = int((pwm_values[6] + 1) * 127.5) - # tcm.vbl = int((pwm_values[7] + 1) * 127.5) tsm = thrust_status_msg() tsm.status = pwm_values + # publish data thrust_pub.publish(tcm) status_pub.publish(tsm) def updateCOM(config, level): rospy.loginfo("""Reconfigure Request: {ROV_X}, {ROV_Y}, {ROV_Z}""".format(**config)) return config - +def _comUpdate(msg): + tm.location = tm.change_origin(msg.com[0],msg.com[1],msg.com[2]) + tm.torque = tm.torque_values() + tm.thruster_force_map = tm.thruster_force_map_values() + rospy.loginfo("changed" + str(msg.com[0]) + ":" + str(msg.com[1]) + ":" + str(msg.com[2])) if __name__ == "__main__": ''' @@ -106,25 +111,22 @@ def updateCOM(config, level): # initialize node and rate rospy.init_node('thrust_control') srv = Server(ROV_COMConfig, updateCOM) - rate = rospy.Rate(50) # 20 hz + rate = rospy.Rate(25) # 20 hz + + thrust_pub = rospy.Publisher('final_thrust', final_thrust_msg, queue_size=10) + status_pub = rospy.Publisher('thrust_status', thrust_status_msg, queue_size=10) # initialize subscribers comm_sub = rospy.Subscriber('/thrust_command', thrust_command_msg, _pilot_command) - #ramp_sub = rospy.Subscriber('/ramp', String, _updateRamp) - # controller_sub = rospy.Subscriber('/surface/controller',controller_msg, _teleop) - #controller_sub = rospy.Subscriber('gamepad_listener', controller_msg, _teleop) + com_sub = rospy.Subscriber('com_tweak', com_msg, _comUpdate) + # initialize publishers - thrust_pub = rospy.Publisher('final_thrust', - final_thrust_msg, queue_size=10) - status_pub = rospy.Publisher('thrust_status', - thrust_status_msg, queue_size=10) # define variable for class Complex to allow calculation of thruster pwm values c = Complex_1.Complex() - tm = thrust_mapping.ThrustMapper() - desired_thrust_final = [0, 0, 0, 0, 0, 0] + + while not rospy.is_shutdown(): - on_loop() - rate.sleep() + rospy.spin() diff --git a/ros/src/control/scripts/thrust_control.py~ b/ros/src/control/scripts/thrust_control.py~ deleted file mode 100755 index 4069e18..0000000 --- a/ros/src/control/scripts/thrust_control.py~ +++ /dev/null @@ -1,109 +0,0 @@ -#! /usr/bin/python -import rospy -from shared_msgs.msg import auto_control_msg, final_thrust_msg, thrust_status_msg, thrust_command_msg, controller_msg -from sensor_msgs.msg import Imu -from std_msgs.msg import Float32 -import numpy as np -import Complex_1 -import thrust_mapping - -desired_p = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -desired_p_unramped = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -locked_dims_list = [False, False, False, False, False, False] -disabled_list = [False, False, False, False, False, False, False, False] -inverted_list = [0, 0, 0, 0, 0, 0, 0, 0] -MAX_CHANGE = .1 -# watch dog stuff -last_packet_time = 0.0 -is_timed_out = False -# timout in ms -WATCHDOG_TIMEOUT = 10 - - -def _pilot_command(comm): - global desired_p # desired thrust from pilot - global disabled_list # disabled thrusters - global inverted_list # inverted thrusters - global desired_p_unramped - print ('new_pilot_data') - desired_p_unramped = comm.desired_thrust - # disabled_list = comm.disable_thrusters - # inverted_list = comm.inverted - - -def ramp(index): - if (abs(desired_p_unramped[index] - desired_p[index]) > MAX_CHANGE): - if (desired_p_unramped[index] - desired_p[index] > 0): - desired_p[index] += MAX_CHANGE - else: - desired_p[index] -= MAX_CHANGE - return - else: - desired_p[index] = desired_p_unramped[index] - - -def on_loop(): - global new_auto_data - global is_timed_out - global last_packet_time - for i in range(0, 6): - ramp(i) - desired_thrust_final[i] = desired_p[i] - - # calculate thrust - #pwm_values = c.calculate(desired_thrust_final, disabled_list, False) - pwm_values = tm.thrustVectorToPWM(tm.calculateThrusterOutput(desired_thrust_final)) - # invert relevant values - #for i in range(8): - # if inverted_list[i] == 1: - # pwm_values[i] = pwm_values[i] * (-1) - - # assign values to publisher messages for thurst control and status - tcm = final_thrust_msg() - # val = float of range(-1, 1) - # if int8: (val * 127.5) - 0.5 will give range -128 to 127 - # if uint8: (val + 1) * 127.5 will give 0 to 255 - tcm.hfl = int((pwm_values[0] + 1) * 127.5) - tcm.hfr = int((pwm_values[1] + 1) * 127.5) - tcm.hbr = int((pwm_values[2] + 1) * 127.5) - tcm.hbl = int((pwm_values[3] + 1) * 127.5) - tcm.vfl = int((pwm_values[4] + 1) * 127.5) - tcm.vfr = int((-1.0*pwm_values[5] + 1) * 127.5) - tcm.vbr = int((pwm_values[6] + 1) * 127.5) - tcm.vbl = int((pwm_values[7] + 1) * 127.5) - - tsm = thrust_status_msg() - tsm.status = pwm_values - # publish data - thrust_pub.publish(tcm) - status_pub.publish(tsm) - - -if __name__ == "__main__": - ''' - Note that this file is only set up for using 8 thrusters. - ''' - - # initialize node and rate - rospy.init_node('thrust_control') - rate = rospy.Rate(50) # 20 hz - - # initialize subscribers - comm_sub = rospy.Subscriber('/thrust_command', thrust_command_msg, _pilot_command) - # controller_sub = rospy.Subscriber('/surface/controller',controller_msg, _teleop) - #controller_sub = rospy.Subscriber('gamepad_listener', controller_msg, _teleop) - # initialize publishers - thrust_pub = rospy.Publisher('final_thrust', - final_thrust_msg, queue_size=10) - status_pub = rospy.Publisher('thrust_status', - thrust_status_msg, queue_size=10) - - # define variable for class Complex to allow calculation of thruster pwm values - c = Complex_1.Complex() - tm = thrust_mapping.ThrustMapper() - desired_thrust_final = [0, 0, 0, 0, 0, 0] - - while not rospy.is_shutdown(): - on_loop() - rate.sleep() - diff --git a/ros/src/control/scripts/thrust_control_simple.py b/ros/src/control/scripts/thrust_control_simple.py deleted file mode 100755 index 59bb010..0000000 --- a/ros/src/control/scripts/thrust_control_simple.py +++ /dev/null @@ -1,162 +0,0 @@ -#! /usr/bin/python -import rospy -from shared_msgs.msg import auto_control_msg, final_thrust_msg, thrust_status_msg, thrust_command_msg -from sensor_msgs.msg import Imu -from std_msgs.msg import Float32 -import numpy as np -from Simple_1 import calculateSimple - -desired_a = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -desired_p = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -desired_p_unramped = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -locked_dims_list = [False, False, False, False, False, False] -disabled_list = [False, False, False, False, False, False, False, False] -inverted_list = [0, 0, 0, 0, 0, 0, 0, 0] -MAX_CHANGE = .1 -# watch dog stuff -last_packet_time = 0.0 -is_timed_out = False -# flags to prevent old data -new_auto_data = False -new_pilot_data = False -# timout in ms -WATCHDOG_TIMEOUT = 10 - - -def _auto_command(msg): - global desired_a # desired thrust from automatic control - global locked_dims_list # locked dimensions - global new_auto_data - print 'new_auto_data' - desired_a = msg.thrust_vec - locked_dims_list = msg.dims_locked - new_auto_data = True - on_loop() - - -def _pilot_command(comm): - global desired_p # desired thrust from pilot - global disabled_list # disabled thrusters - global inverted_list # inverted thrusters - global new_pilot_data - global desired_p_unramped - print 'new_pilot_data' - desired_p_unramped = comm.desired_thrust - for i in range(0, 6): - ramp(i) - disabled_list = comm.disable_thrusters - inverted_list = comm.inverted - new_pilot_data = True - on_loop() - - -def ramp(index): - if (abs(desired_p_unramped[index] - desired_p[index]) > MAX_CHANGE): - if (desired_p_unramped[index] - desired_p[index] > 0): - desired_p[index] += MAX_CHANGE - else: - desired_p[index] -= MAX_CHANGE - return - else: - desired_p[index] = desired_p_unramped[index] - - -def on_loop(): - global new_pilot_data - global new_auto_data - global is_timed_out - global last_packet_time - - # check to see if you have new data - if (not (new_pilot_data and new_auto_data) and not is_timed_out): - return - # reset flags and execute - if new_pilot_data and new_auto_data: - is_timed_out = False - print "on_loop p=" + str(new_pilot_data) + " a=" + str(new_auto_data) + " t=" + str(is_timed_out) - new_auto_data = False - new_pilot_data = False - if (not is_timed_out): - # reset the watchdog timer - curr_time = rospy.get_rostime() - last_packet_time = curr_time.secs + curr_time.nsecs * 10 ** -9 - - for i in range(6): - if (is_timed_out): - desired_thrust_final[i] = 0.0 - else: - # if dimension locked, set desired thrust to auto; else set to pilot controls - if locked_dims_list[i] == True: - desired_thrust_final[i] = desired_a[i] - else: - desired_thrust_final[i] = desired_p[i] - - # calculate thrust - pwm_values = calculateSimple(desired_thrust_final) - # invert relevant values - for i in range(8): - if inverted_list[i] == 1: - pwm_values[i] = pwm_values[i] * (-1) - - # assign values to publisher messages for thurst control and status - tcm = final_thrust_msg() - # val = float of range(-1, 1) - # if int8: (val * 127.5) - 0.5 will give range -128 to 127 - # if uint8: (val + 1) * 127.5 will give 0 to 255 - tcm.hfl = int((pwm_values[0] + 1) * 127.5) - tcm.hfr = int((pwm_values[1] + 1) * 127.5) - tcm.hbr = int((pwm_values[2] + 1) * 127.5) - tcm.hbl = int((pwm_values[3] + 1) * 127.5) - tcm.vfl = int((pwm_values[4] + 1) * 127.5) - tcm.vfr = int((pwm_values[5] + 1) * 127.5) - tcm.vbr = int((pwm_values[6] + 1) * 127.5) - tcm.vbl = int((pwm_values[7] + 1) * 127.5) - - tsm = thrust_status_msg() - tsm.status = pwm_values - - # publish data - thrust_pub.publish(tcm) - status_pub.publish(tsm) - - -if __name__ == "__main__": - ''' - Note that this file is only set up for using 8 thrusters. - ''' - - # initialize node and rate - rospy.init_node('thrust_control') - rate = rospy.Rate(10) # 10 hz - - # initialize subscribers - auto_sub = rospy.Subscriber('auto_control', - auto_control_msg, _auto_command) - comm_sub = rospy.Subscriber('/surface/thrust_command', - thrust_command_msg, _pilot_command) - - # initialize publishers - thrust_pub = rospy.Publisher('final_thrust', - final_thrust_msg, queue_size=10) - status_pub = rospy.Publisher('thrust_status', - thrust_status_msg, queue_size=10) - - # define variable for class Complex to allow calculation of thruster pwm values - # c = Complex_1.Complex() - desired_thrust_final = [0, 0, 0, 0, 0, 0] - - while not rospy.is_shutdown(): - compare_time = rospy.get_rostime() - compare_time = compare_time.secs + compare_time.nsecs * 10 ** -9 - print str(compare_time) + " : " + str(last_packet_time) - if (compare_time - last_packet_time > WATCHDOG_TIMEOUT): - is_timed_out = True - if (is_timed_out): - # global disabled_list - # disabled_list = [True, True, True, True, True, True, True, True] - on_loop() - curr_time = rospy.get_rostime() - last_packet_time = curr_time.secs + curr_time.nsecs * 10 ** -9 - is_timed_out = False - - rate.sleep() diff --git a/ros/src/control/scripts/thrust_mapping.py b/ros/src/control/scripts/thrust_mapping.py index d4a7f49..a8b03b1 100644 --- a/ros/src/control/scripts/thrust_mapping.py +++ b/ros/src/control/scripts/thrust_mapping.py @@ -1,236 +1,114 @@ import numpy as np -from numpy import linalg - -# Measurements ripped from Complex_1.py. We'll probably need to remeasure and calibrate a lot of these, -# then this comment should be updated describing our process and where all the numbers came from. -# -# All measurements are based on: -# X: LEFT/RIGHT MOTION OF THE ROV. LEFT = +X -# Y: FORWARD/BACK. FORWARD = +Y = SIDE THE ROV CLAW IS ON -# Z: UP/DOWN. UP = +Z -# -# PITCH: TILT FORWARD/BACK. +PITCH TILTS FORWARD (LOOK DOWN). USE RIGHT HAND RULE ON X AXIS. -# ROLL: ROLL LEFT/RIGHT. +ROLL ROLLS LEFT. USE NEGATIVE RIGHT HAND RULE ON Y AXIS. -# YAW: ROTATE LEFT/RIGHT. +YAW ROTATES RIGHT. USE NEGATIVE RIGHT HAND RULE ON Z AXIS. -# - -# When a percent power is requested, this direction scale is used to calculate the output vector. The reason -# for scaling this is that when xxx motion is requested in one direction and yyy motion is requested in another, -# the ROV might be much stronger in one of those directions. e.g., the ROV can move up/down very fast, so if we -# request 100% forward and 100% up, the ROV will move almost entirely upward. This can get real wonky with rotation -# too. To make a direction stronger, increase its number here, and to make it weaker, decrease its number here. -# Keep them all real numbers >= 1. -# X Y Z P R Y -DIRECTION_SCALE = [10, 10, 5, 5, 5, 5] - -# How similar desired and output vectors must be. If a desired force is not possible, algo will pump out as much power -# as possible in the exact direction. Then, it will add in additional force if possible, as long as any additional force -# is close enough to the needed additional force. Value is scaled from 0 to 1, 1 for exact vectors only, and 0 for -# fuckit let's go places. -SIMILARITY_MINIMUM = .95 -# If we already yield >= this amount of the desired force, then break -PERCENT_DESIRED_FORCE_YIELDED = .99 -# Values beneath this are assumed to be zero because of double rounding -ZERO_ROUND_THRESHOLD = .0001 -# Thruster thrust limits. THRUST_MIN is max reverse thrust -THRUST_MIN = -2.9 -THRUST_MAX = 3.71 -# Center of mass coordinates relative to our measurement point -COM_X = 0.0 # 0.056 * 0.0254 -COM_Y = 0.0 # -0.1256 * 0.0254 -COM_Z = 2.0 * 0.0254 - -# Thruster locations relative to the measurement point of the ROV. -# X Y Z -location = np.matrix([[4.438, 5.679, 0], # Thruster 1 - [-4.438, 5.679, 0], # Thruster 2 - [-4.438, -5.679, 0], # Thruster 3 - [4.438, -5.679, 0], # Thruster 4 - [7.5, 7.313, -2.25], # Thruster 5 - [-7.5, 7.313, -2.25], # Thruster 6 - [-7.5, -7.313, -2.25], # Thruster 7 - [7.5, -7.313, -2.25]]) # Thruster 8 -location *= 0.0254 - -# Directions the thrust forces point. MUST BE UNIT VECTORS. -# NOTE -# THIS IS NOT THE DIRECTIONS THE THRUSTERS POINT. IT IS THE DIRECTION IN WHICH POSITIVE FORCE IS DEFINED FOR A THRUSTER. - -XCOMP = np.sin(7 * np.pi / 18) -YCOMP = np.cos(7 * np.pi / 18) - -# X Y Z -direction = np.matrix([[0, 0, 1], # Thruster 1 - [0, 0, 1], # Thruster 2 - [0, 0, 1], # Thruster 3 - [0, 0, 1], # Thruster 4 - [XCOMP, -YCOMP, 0], # Thruster 5 - [-XCOMP, -YCOMP, 0], # Thruster 6 - [-XCOMP, YCOMP, 0], # Thruster 7 - [XCOMP, YCOMP, 0]]) # Thruster 8 - -# Assert all direction vectors are unit vectors (+- 1% for rounding) -for vector in direction: - assert abs(vector[0, 0] ** 2 + vector[0, 1] ** 2 + vector[0, 2] ** 2 - 1) < .01, 'DIRECTION VECTORS MUST BE UNIT VECTORS' +SCALE = 0.0254 +THRUST_MAX = 3.71 +THRUST_MIN = -2.9 +X_COMP = np.sin(7 * np.pi / 18) +Y_COMP = np.cos(7 * np.pi / 18) +is_inverted = [True,False,True,True,False,False,True,False] class ThrustMapper: def __init__(self): - self.location = np.copy(location) - self.direction = np.copy(direction) - self.torque = None - self.changeOriginLocation(COM_X, COM_Y, COM_Z) - self.thrusterForceMap = None - self.createThruserForceMap() - - # Changes the origin location to the given location. - # relX = the new X origin relative to the current X origin - # relY = the new Y origin relative to the current Y origin - # relZ = the new Z origin relative to the current Z origin - def changeOriginLocation(self, relX, relY, relZ): - for i in range(0, len(self.location)): - self.location[i][0] = self.location[i][0] - relX - self.location[i][1] = self.location[i][1] - relY - self.location[i][2] = self.location[i][2] - relZ - - self.calcTorqueValues() - - # Calculates the torque matrix using the location matrix and the direction matrix. - # Resulting torque values be an 3x8 array with a [pitch, roll, yaw] entry for each thruster. - def calcTorqueValues(self): - torque = [] - for i in range(0, len(self.location)): - # PITCH = Ly * Dz - Lz * Dy - # ROLL = -Lx * Dz + Lz * Dx - # YAW = Lx * Dy - Ly * Dx - torque.append(np.cross(self.location[i], self.direction[i])) - self.torque = np.matrix(torque) - - # Concatenates the direction and torque vectors into a single array - # Result is an 6x8 array. One column per thruster, one row per direction/torque. Rows are ordered x, y, z, roll, pitch, yaw. - # Columns are ordered by thruster - def createThruserForceMap(self): + self.com = np.array([0.0, 0.0, 1.4]) * SCALE + self.location_frame_absolute = np.matrix([[7.328, 7.216, 6.839], # Thruster 1 + [-7.328, 7.216, 6.839], # Thruster 2 + [-7.328, -7.216, 6.839], # Thruster 3 + [7.328, -7.216, 6.839], # Thruster 4 + [7.328, 7.216, -2.839], # Thruster 5 + [-7.328, 7.216, -2.839], # Thruster 6 + [-7.328, -7.216, -2.839], # Thruster 7 + [7.328, -7.216, -2.839]]) * SCALE # Thruster 8 + alpha = 30 * np.pi / 180.0 + beta = 30 * np.pi / 180.0 + x_comp = np.cos(alpha) * np.cos(beta) + y_comp = np.sin(alpha) * np.cos(beta) + z_comp = np.sin(beta) + + + self.direction = np.matrix([[ x_comp, -y_comp, -z_comp], # Thruster 1 + [-x_comp, -y_comp, -z_comp], # Thruster 2 + [-x_comp, y_comp, -z_comp], # Thruster 3 + [ x_comp, y_comp, -z_comp], # Thruster 4 + [ x_comp, -y_comp, z_comp], # Thruster 5 + [-x_comp, -y_comp, z_comp], # Thruster 6 + [-x_comp, y_comp, z_comp], # Thruster 7 + [ x_comp, y_comp, z_comp]]) # Thruster 8 + + self.location = self.change_origin(0, 0, 0) + self.torque = self.torque_values() + self.thruster_force_map = self.thruster_force_map_values() + + self.fine = True + self.multiplier = 1.041 + + def set_fine(self, nfine): + self.fine = nfine + + def set_multiplier(self, nmul): + self.multiplier = nmul + + def change_origin(self, x, y, z): + return self.location_frame_absolute - self.com + np.array([x, y, z]) * SCALE + + def torque_values(self): + return np.cross(self.location, self.direction) + + def thruster_force_map_values(self): assert len(self.direction) == len(self.torque), 'Initialize direction and torque first!' - self.thrusterForceMap = np.concatenate((np.transpose(self.direction), np.transpose(self.torque))) - - # Calculates thruster output. Desired force should be a 6-dimensional vector, x, y, z, roll, pitch, yaw - # Output is an 8-d vector of the thrust of each thruster - def calculateThrusterOutput(self, desiredForce): - thrustLeft = np.copy(self.thrusterForceMap) - outputNeeded = np.transpose(np.matrix((desiredForce,), dtype=np.float)) - outputThrust = np.transpose(np.matrix(np.zeros((len(location))), dtype=np.float)) - - while thrustLeft.size > 0: - psuedoInv = linalg.pinv(thrustLeft) - - thrusterForceIteration = np.matmul(psuedoInv, outputNeeded) - - additionalOutput = np.matmul(self.thrusterForceMap, thrusterForceIteration) - if np.linalg.norm(outputNeeded) < ZERO_ROUND_THRESHOLD: - break - - # If accurate additional force not possible, break - similarity = np.dot(np.transpose(outputNeeded), additionalOutput) / (np.linalg.norm(outputNeeded) * np.linalg.norm(additionalOutput)) - - if similarity < SIMILARITY_MINIMUM: - break - - # Scale down force if overloading a thruster - scaleDownMultiplier = 1 - scaleDownIndex = -1 - for i in range(0, thrusterForceIteration.size): - currentOutput = outputThrust[i] - additionalOutput = thrusterForceIteration[i] - - if abs(additionalOutput) <= ZERO_ROUND_THRESHOLD: - continue - thrusterScale = 1 - if currentOutput + additionalOutput > THRUST_MAX: - thrusterScale = (THRUST_MAX - currentOutput) / additionalOutput - if currentOutput + additionalOutput < THRUST_MIN: - thrusterScale = (THRUST_MIN - currentOutput) / additionalOutput - if thrusterScale < scaleDownMultiplier: - scaleDownMultiplier = thrusterScale - scaleDownIndex = i - - # If a thruster is maxed, disable it in further calculation - if scaleDownIndex != -1: - thrusterForceIteration *= scaleDownMultiplier - # print('Thrust left: ') - # print(thrustLeft) - # print('Zeroing column %d' % scaleDownIndex) - thrustLeft[:, scaleDownIndex] = 0 - # print('New thrust left:' ) - # print(thrustLeft) - - outputThrust += thrusterForceIteration - outputNeeded -= np.matmul(self.thrusterForceMap, thrusterForceIteration) - # print('Thrust calculated: ') - # print(outputThrust) - # print('Output still needed: ') - # print(outputNeeded) - if np.linalg.norm(outputNeeded) < np.linalg.norm(desiredForce) * (1 - PERCENT_DESIRED_FORCE_YIELDED): - break - # print('Restarting!!!!!!!!!!!!!!!!') - - return np.transpose(outputThrust).tolist()[0] - - # Have all percents 0-1. 6-d vector, just like calculateThrusterOutput - # Converts desired percent into a weighted thrust vector in the desired direction. To find 100% thrust - # in that direction, multiply desired by a huge constant then calcualate thrust. Once the 100% thrust vector - # is found, multiply by the desired percent in each direction - def calculateThrusterOutputFromPercent(self, desiredPercent): - BIG_CONSTANT = 1000000 - scaledThrust = [] - for i in range(0, len(desiredPercent)): - scaledThrust.append(desiredPercent[i] * DIRECTION_SCALE[i] * BIG_CONSTANT) - maxThrustInDirection = self.calculateThrusterOutput(scaledThrust) - - # Assume the largest percent is leading the direction, scale down by largest percent in vector - for i in range(0, len(maxThrustInDirection)): - maxThrustInDirection *= max(desiredPercent) - - return maxThrustInDirection - - def thrusterOutputToNetForce(self, output): - return np.matmul(self.thrusterForceMap, np.transpose(np.array(output))).tolist() - - def thrustToTotalForce(self, thrust): - return np.matmul(self.thrusterForceMap, thrust) - - # Blindly copied from Complex_1. Check these values at some point please - def thrustToPWM(self, thrustVal): - if thrustVal < -ZERO_ROUND_THRESHOLD: - pwm = 0.018 * (thrustVal ** 3) + 0.117 * (thrustVal ** 2) + 0.4955 * thrustVal - 0.0991 - elif thrustVal > ZERO_ROUND_THRESHOLD: - pwm = 0.0095 * (thrustVal ** 3) - 0.0783 * (thrustVal ** 2) + 0.4004 * thrustVal + 0.0986 + return np.concatenate((np.transpose(self.direction), np.transpose(self.torque))) + + def thruster_output(self, desired_force): + if not np.array_equal(desired_force, np.zeros(6)): + if np.linalg.norm(desired_force) > 1.5: + desired_force /= np.linalg.norm(desired_force) + desired_force *= 1.5 + + output_needed = np.transpose(np.array((desired_force,), dtype=np.float)) + psuedo_inv = np.linalg.pinv(self.thruster_force_map) + force = np.matmul(psuedo_inv, output_needed) + if self.fine: + force *= self.multiplier #1.041 # 3.7657 # Thrust envelop inscribed sphere radius + else: + scale_max = abs(THRUST_MAX / max(force)) + scale_min = abs(THRUST_MIN / min(force)) + force *= min(scale_max, scale_min) + for i in range(0,7): + if(is_inverted[i]): + force[i] *= -1.0 + return np.transpose(force).tolist()[0] + return np.zeros(8) + + @staticmethod + def thrust_to_pwm(thrust_val): + if thrust_val < -.04: + pwm = 0.018 * (thrust_val ** 3) + 0.117 * (thrust_val ** 2) + 0.4981 * thrust_val - 0.09808 + elif thrust_val > .04: + pwm = 0.0095 * (thrust_val ** 3) - 0.0783 * (thrust_val ** 2) + 0.4004 * thrust_val + 0.0986 else: - # assume 0 even though dead band has range of pwm values - pwm = 0 + pwm = 0.0 return pwm - def PMWToThrust(self, pwm): - thrust = 0.0 + + @staticmethod + def pwm_to_thrust(pwm): if pwm < -.1: thrust = -.8944 * (pwm ** 3) - 2.971 * (pwm ** 2) + 0.9844 * pwm + .1005 elif pwm > .1: - thrust = -1.1095* (pwm ** 3) + 3.9043 * (pwm ** 2) + 1.1101 * pwm - 0.113 + thrust = -1.1095 * (pwm ** 3) + 3.9043 * (pwm ** 2) + 1.1101 * pwm - 0.113 else: - # assume 0 even though dead band has range of pwm values - thrust = 0 + thrust = 0.0 return thrust - def PWMVectorToThrust(self, PWMVector): - return list(self.PMWToThrust(i) for i in PWMVector) - def thrustVectorToPWM(self, thrustVector): - return list(self.thrustToPWM(i) for i in thrustVector) if __name__ == '__main__': - tm = ThrustMapper() - - for i in range(100): - desired_thrust_final = [0, 0, 0, 0, 0, 0] # X Y Z Ro Pi Ya - - pwm_values = tm.calculateThrusterOutput(desired_thrust_final) - - print(list(np.around(np.array(pwm_values), 2))) + # global oneiteration + tm = ThrustMapper() # for i in range(100): + desired_thrust_final = [0.1, 0, 0.0, 0, 0, 0] # X Y Z Ro Pi Ya pwm_values = tm.thruster_output(desired_thrust_final) + # oneiteration = True + pwm_values2 = tm.thruster_output(desired_thrust_final) + result1 = np.matmul(tm.thrusterForceMap, pwm_values) + result2 = np.matmul(tm.thrusterForceMap, pwm_values2) + print(list(np.around(np.array(pwm_values), 2))) + print(list(np.around(np.array(pwm_values2), 2))) + print(list(np.around(np.array(result1), 2))) + print(list(np.around(np.array(result2), 2))) diff --git a/ros/src/control/scripts/thrust_mapping.pyc b/ros/src/control/scripts/thrust_mapping.pyc deleted file mode 100644 index 15bf689..0000000 Binary files a/ros/src/control/scripts/thrust_mapping.pyc and /dev/null differ diff --git a/ros/src/control/scripts/thrust_mapping_dev.py b/ros/src/control/scripts/thrust_mapping_dev.py new file mode 100644 index 0000000..529cfdb --- /dev/null +++ b/ros/src/control/scripts/thrust_mapping_dev.py @@ -0,0 +1,105 @@ +import numpy as np + +SCALE = 0.0254 + +THRUST_MAX = 3.71 +THRUST_MIN = -2.9 + +X_COMP = np.sin(7 * np.pi / 18) +Y_COMP = np.cos(7 * np.pi / 18) + +class ThrustMapper: + def __init__(self): + self.com = np.array([0.0, 0.0, 1.4]) * SCALE + self.location_frame_absolute = np.array([[+4.438, 5.679, 0.00], # Thruster 1 + [-4.438, 5.679, 0.00], # Thruster 2 + [-4.438, -5.679, 0.00], # Thruster 3 + [+4.438, -5.679, 0.00], # Thruster 4 + [+7.500, 7.313, -2.25], # Thruster 5 + [-7.500, 7.313, -2.25], # Thruster 6 + [-7.500, -7.313, -2.25], # Thruster 7 + [+7.500, -7.313, -2.25]]) * SCALE # Thruster 8 + + self.direction = np.array([[0, 0, 1], # Thruster 1 + [0, 0, 1], # Thruster 2 + [0, 0, 1], # Thruster 3 + [0, 0, 1], # Thruster 4 + [+X_COMP, -Y_COMP, 0], # Thruster 5 + [-X_COMP, -Y_COMP, 0], # Thruster 6 + [-X_COMP, Y_COMP, 0], # Thruster 7 + [+X_COMP, Y_COMP, 0]]) # Thruster 8 + + self.location = self.change_origin(0, 0, 0) + self.torque = self.torque_values() + self.thruster_force_map = self.thruster_force_map_values() + + self.fine = True + self.multiplier = 1.041 + + def set_fine(self, nfine): + self.fine = nfine + + def set_multiplier(self, nmul): + self.multiplier = nmul + + def change_origin(self, x, y, z): + return self.location_frame_absolute - self.com + np.array([x, y, z]) * SCALE + + def torque_values(self): + return np.cross(self.location, self.direction) + + def thruster_force_map_values(self): + assert len(self.direction) == len(self.torque), 'Initialize direction and torque first!' + return np.concatenate((np.transpose(self.direction), np.transpose(self.torque))) + + def thruster_output(self, desired_force): + if not np.array_equal(desired_force, np.zeros(6)): + if np.linalg.norm(desired_force) > 1.5: + desired_force /= np.linalg.norm(desired_force) + desired_force *= 1.5 + + output_needed = np.transpose(np.array((desired_force,), dtype=np.float)) + psuedo_inv = np.linalg.pinv(self.thruster_force_map) + force = np.matmul(psuedo_inv, output_needed) + if self.fine: + force *= self.multiplier #1.041 # 3.7657 # Thrust envelop inscribed sphere radius + else: + scale_max = abs(THRUST_MAX / max(force)) + scale_min = abs(THRUST_MIN / min(force)) + force *= min(scale_max, scale_min) + return np.transpose(force).tolist()[0] + return np.zeros(8) + + @staticmethod + def thrust_to_pwm(thrust_val): + if thrust_val < -.04: + pwm = 0.018 * (thrust_val ** 3) + 0.117 * (thrust_val ** 2) + 0.4981 * thrust_val - 0.09808 + elif thrust_val > .04: + pwm = 0.0095 * (thrust_val ** 3) - 0.0783 * (thrust_val ** 2) + 0.4004 * thrust_val + 0.0986 + else: + pwm = 0.0 + return pwm + + @staticmethod + def pwm_to_thrust(pwm): + if pwm < -.1: + thrust = -.8944 * (pwm ** 3) - 2.971 * (pwm ** 2) + 0.9844 * pwm + .1005 + elif pwm > .1: + thrust = -1.1095 * (pwm ** 3) + 3.9043 * (pwm ** 2) + 1.1101 * pwm - 0.113 + else: + thrust = 0.0 + return thrust + + +if __name__ == '__main__': + # global oneiteration + tm = ThrustMapper() # for i in range(100): + desired_thrust_final = [0.1, 0, 0.0, 0, 0, 0] # X Y Z Ro Pi Ya pwm_values = tm.thruster_output(desired_thrust_final) + # oneiteration = True + pwm_values2 = tm.thruster_output(desired_thrust_final) + result1 = np.matmul(tm.thrusterForceMap, pwm_values) + result2 = np.matmul(tm.thrusterForceMap, pwm_values2) + print(list(np.around(np.array(pwm_values), 2))) + print(list(np.around(np.array(pwm_values2), 2))) + print(list(np.around(np.array(result1), 2))) + print(list(np.around(np.array(result2), 2))) diff --git a/ros/src/control/scripts/thrust_proc.py b/ros/src/control/scripts/thrust_proc.py deleted file mode 100644 index e69de29..0000000 diff --git a/ros/src/control/scripts/thruster_control.py b/ros/src/control/scripts/thruster_control.py deleted file mode 100644 index e69de29..0000000 diff --git a/ros/src/i2c/scripts/imu_proc.py b/ros/src/i2c/scripts/imu_proc.py index 1a5d098..8dfe9ee 100755 --- a/ros/src/i2c/scripts/imu_proc.py +++ b/ros/src/i2c/scripts/imu_proc.py @@ -8,8 +8,8 @@ from std_msgs.msg import Bool from geometry_msgs.msg import Pose import geometry_msgs.msg -from tf.transformations import euler_from_quaternion -import tf2_ros +#from tf.transformations import euler_from_quaternion +#import tf2_ros from pyquaternion import Quaternion IMU_PITCH_OFFSET = 0.0 @@ -45,7 +45,7 @@ def quaternion_multiply(quaternion1, quaternion0): -x1*z0 + y1*w0 + z1*x0 + w1*y0, x1*y0 - y1*x0 + z1*w0 + w1*z0]) def publishTF(self): - br = tf2_ros.TransformBroadcaster() + #br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() @@ -59,7 +59,7 @@ def publishTF(self): t.transform.rotation.z = imu.quat_arr()[2] t.transform.rotation.w = imu.quat_arr()[3] - br.sendTransform(t) + #br.sendTransform(t) if __name__ == "__main__": global imu rospy.init_node('imu_proc') @@ -70,15 +70,15 @@ def publishTF(self): pub2 = rospy.Publisher('imu_quat', Pose, queue_size=1) - tfBuffer = tf2_ros.Buffer() - listener = tf2_ros.TransformListener(tfBuffer) + #tfBuffer = tf2_ros.Buffer() + #listener = tf2_ros.TransformListener(tfBuffer) rate = rospy.Rate(10) #sub = rospy.Subscriber('reset_imu', Bool, # _reset_imu_offsets) - rate = rospy.Rate(50) # 10hz + rate = rospy.Rate(25) # 10hz while not rospy.is_shutdown(): # try: # trans = tfBuffer.lookup_transform('imu', 'base_link', rospy.Time()) @@ -125,11 +125,12 @@ def publishTF(self): #T3 = Quaternion([ 0.7068252, 0, 0, 0.7073883 ]) #resultQ = quaternion_multiply(transQ, imuQ) R = Q * T1 - rads = euler_from_quaternion(Q.elements) + #rads = euler_from_quaternion(Q.elements) out_message.header.stamp = rospy.Time.now() - out_message.gyro - for i in range(0,3): - out_message.gyro[i] = rads[i] * 180.0 / 3.1415 + out_message.header.frame_id = "/IMU_euler" + #out_message.gyro + #for i in range(0,3): + # out_message.gyro[i] = rads[i] * 180.0 / 3.1415 # = euler_from_quaternion(imu.quat_arr())#[imu.quat_x(),imu.quat_y(),imu.quat_z()] # out_message.gyro = [imu.temp(),imu.temp(),imu.temp()] ROV_X_Accel = imu.acceleration_z() diff --git a/ros/src/i2c/scripts/imu_proc.py.save b/ros/src/i2c/scripts/imu_proc.py.save new file mode 100755 index 0000000..1ead77c --- /dev/null +++ b/ros/src/i2c/scripts/imu_proc.py.save @@ -0,0 +1,140 @@ +#! /usr/bin/python3 +import rospy +import smbus +import math +import numpy as np +from BNO055 import BNO055 +from shared_msgs.msg import imu_msg +from std_msgs.msg import Bool +from geometry_msgs.msg import Pose +import geometry_msgs.msg +from tf.transformations import euler_from_quaternion +import tf2_ros +from pyquaternion import Quaternion + +IMU_PITCH_OFFSET = 0.0 +IMU_ROLL_OFFSET = 0.0 +IMU_YAW_OFFSET = 0.0 + +def reset_imu_offsets(): + global imu + global IMU_PITCH_OFFSET + global IMU_ROLL_OFFSET + global IMU_YAW_OFFSET + print ("message recieved") + IMU_PITCH_OFFSET = imu.pitch() + IMU_ROLL_OFFSET = imu.roll() + IMU_YAW_OFFSET = imu.yaw() + print ("imu_pitch offset" , IMU_PITCH_OFFSET) + +#bind all angles to -180 to 180 +def clamp_angle_neg180_to_180(angle): + angle_0_to_360 = clamp_angle_0_to_360(angle) + if angle_0_to_360 > 180: + return angle_0_to_360 - 180 * -1.0 + return angle_0_to_360 +#bind all angles to -180 to 180 +def clamp_angle_0_to_360(angle): + return (angle + 1 * 360) - math.floor((angle + 2 * 360)/360)*360 + +def quaternion_multiply(quaternion1, quaternion0): + w0, x0, y0, z0 = quaternion0 + w1, x1, y1, z1 = quaternion1 + return np.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0, + x1*w0 + y1*z0 - z1*y0 + w1*x0, + -x1*z0 + y1*w0 + z1*x0 + w1*y0, + x1*y0 - y1*x0 + z1*w0 + w1*z0]) +def publishTF(self): + br = tf2_ros.TransformBroadcaster() + t = geometry_msgs.msg.TransformStamped() + + t.header.stamp = rospy.Time.now() + t.child_frame_id = "imu" + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + #q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta) + t.transform.rotation.x = imu.quat_arr()[0] + t.transform.rotation.y = imu.quat_arr()[1] + t.transform.rotation.z = imu.quat_arr()[2] + t.transform.rotation.w = imu.quat_arr()[3] + + br.sendTransform(t) +if __name__ == "__main__": + global imu + rospy.init_node('imu_proc') + imu = BNO055() + # Publish to the CAN hardware transmitter + pub = rospy.Publisher('imu', imu_msg, + queue_size=1) + pub2 = rospy.Publisher('imu_quat', Pose, + queue_size=1) + + tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(tfBuffer) + rate = rospy.Rate(10) + + + #sub = rospy.Subscriber('reset_imu', Bool, + # _reset_imu_offsets) + + rate = rospy.Rate(50) # 10hz + while not rospy.is_shutdown(): + # try: + # trans = tfBuffer.lookup_transform('imu', 'base_link', rospy.Time()) + # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + # rate.sleep() + # print("could not find it") + # continue + if imu.update(): + out_message = imu_msg() + pose_message = Pose() + + # br = tf2_ros.TransformBroadcaster() + # t = geometry_msgs.msg.TransformStamped() + + # t.header.stamp = rospy.Time.now() + # t.header.frame_id = "world" + # t.child_frame_id = "imu" + # t.transform.translation.x = 0.0 + # t.transform.translation.y = 0.0 + # t.transform.translation.z = 0.0 + # #q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta) + # t.transform.rotation.x = imu.quat_arr()[0] + # t.transform.rotation.y = imu.quat_arr()[1] + # t.transform.rotation.z = imu.quat_arr()[2] + # t.transform.rotation.w = imu.quat_arr()[3] + + # br.sendTransform(t) + + # convert everything to a 0 to 360 to apply a 1d rotation then convert back to -180 to 180 + ROV_Pitch = clamp_angle_0_to_360(imu.roll()) - IMU_ROLL_OFFSET + ROV_Roll = clamp_angle_0_to_360(imu.yaw()) - IMU_YAW_OFFSET + ROV_Yaw = clamp_angle_0_to_360(imu.pitch()) - IMU_PITCH_OFFSET + # out_message.gyro = [ROV_Pitch, ROV_Roll, ROV_Yaw] + pose_message.orientation.x = imu.quat_x() + pose_message.orientation.y = imu.quat_y() + pose_message.orientation.z = imu.quat_z() + pose_message.orientation.w = imu.quat_w() + pub2.publish(pose_message) + transQ = np.array([0, 0.7068252, 0, 0.7073883])#[trans.transform.rotation.x,trans.transform.rotation.y,trans.transform.rotation.z,trans.transform.rotation.w]) + imuQ = np.array(imu.quat_arr()) + Q = Quaternion(imu.quat_arr()) + T1 = Quaternion(axis=[1, 0, 0], angle=(90 * 3.141592/180)) + T2 = Quaternion(axis=[0,0,1], angle=(-90 *3.141592/180.0)) + #T3 = Quaternion([ 0.7068252, 0, 0, 0.7073883 ]) + #resultQ = quaternion_multiply(transQ, imuQ) + R = Q * T1 + rads = euler_from_quaternion(Q.elements) + out_message.gyr + for i in range(0,3): + out_message.gyro[i] = rads[i] * 180.0 / 3.1415 + # = euler_from_quaternion(imu.quat_arr())#[imu.quat_x(),imu.quat_y(),imu.quat_z()] + # out_message.gyro = [imu.temp(),imu.temp(),imu.temp()] + ROV_X_Accel = imu.acceleration_z() + ROV_Y_Accel = imu.acceleration_x() + ROV_Z_Accel = imu.acceleration_y() + out_message.accel = [ROV_X_Accel, ROV_Y_Accel, ROV_Z_Accel] + pub.publish(out_message) + rate.sleep() + diff --git a/ros/src/i2c/scripts/nuked_dumb_servo_listener.py b/ros/src/i2c/scripts/nuked_dumb_servo_listener.py new file mode 100755 index 0000000..8073ef0 --- /dev/null +++ b/ros/src/i2c/scripts/nuked_dumb_servo_listener.py @@ -0,0 +1,94 @@ +#! /usr/bin/env python3 + +#Mom use this + +#set servo angle to certain value and test to see if imu works with moving angle +#then test without imu to see if actual servo has basic functionality +#then test with imu enabled to see servo will actually move with it +#then hook up to front end and see what clicking lock and unlock mid difference does + + +#Notes: +#This scripts takes an input of 0-180 degrees from the test servo publisher node +# It uses a math equation to translate this value into a number with a range of 1-12 +#The servo specifications indicate a duty cycle of 1-2ms with a total period of 20ms. This would indicate a duty cycle from 5-10 percentage wise. +#However for some reason testing hardcoded values found that it takes 1-12 value for a full range of 180 degree motion for some reason +#that's not mathmatically correct but it works so eh +#duty cycle(6) for in the middle. duty cycle(0) is off +#for this inclosure the input can be 12 to 120 degrees. the code will automatically make these the max values + + +#import statements +import RPi.GPIO as GPIO #imports the standard Raspberry Pi GPIO library +import rospy +from time import sleep #imports sleep (aka waiting or pause) into the program +from shared_msgs.msg import servo_msg + +MAX_ANGLE = 98 +MIN_ANGLE = 20 + +#Coverts angle to duty cycle, need to update values here +def angleToDuty(angle): + duty = ((angle /180) * 11) + 1 #converts to duty cycle + return duty + +#initialization +GPIO.setmode(GPIO.BCM) #Setting a Pin Mode aka Chose Board pin number scheme +#Set up pin 14 for PWM +GPIO.setup(13, GPIO.OUT) #sets up pin 13 to an output +p = GPIO.PWM(13,50) #sets up pin 13 as a PWM pin(50 is the frequency) +p.start(0) #starts running PWM on the pin and sets it to 0. 0 is the middle +angle_prev = 45 #sets to middle angle +duty_prev = angleToDuty(angle_prev) +p.ChangeDutyCycle(duty_prev) +sleep(0.04) #max time delay +p.ChangeDutyCycle(0) + +def callback(servoStuff): + #print(imuStuff) + global angle_prev + global duty_prev + + adjustedAngle = servoStuff.angle + if adjustedAngle > MAX_ANGLE: + adjustedAngle = MAX_ANGLE + elif adjustedAngle < MIN_ANGLE: + adjustedAngle = MIN_ANGLE + + if (adjustedAngle != angle_prev): + adjustedDuty = angleToDuty(adjustedAngle) + + dutyDiff = abs(duty_prev - adjustedDuty) + timeToWait = (12.5*(dutyDiff**0.515)+10)/1000 + + + print(f'Old Adjusted angle: {angle_prev}') + print(f'Old Adjusted duty: {duty_prev}') + print(f'Adjusted angle: {adjustedAngle}') + print(f'Adjusted duty: {adjustedDuty}') + print("\n") + p.ChangeDutyCycle(adjustedDuty) + duty_prev = adjustedDuty + angle_prev = adjustedAngle + sleep(timeToWait) #this is so the servo pauses before turning off the power + p.ChangeDutyCycle(0) #if the servo jitters a lot uncomment this it should stop all movement in between calls + +def listener(): + + # In ROS, nodes are uniquely named. If two nodes with the same + # name are launched, the previous one is kicked off. The + # anonymous=True flag means that rospy will choose a unique + # name for our 'listener' node so that multiple listeners can + # run simultaneously. + print("starting") + + rospy.Subscriber('ServoAngles', servo_msg, callback) + rospy.spin() # spin() simply keeps python from exiting until this node is stopped + + print("shutting down servo node") + + +if __name__ == '__main__': + rospy.init_node('listener', anonymous=True) + listener() + diff --git a/ros/src/i2c/scripts/servo_listener.py b/ros/src/i2c/scripts/servo_listener.py index 3b8d086..a064ec4 100755 --- a/ros/src/i2c/scripts/servo_listener.py +++ b/ros/src/i2c/scripts/servo_listener.py @@ -1,25 +1,98 @@ -#! /usr/bin/python3 +#! /usr/bin/env python3 + +#set servo angle to certain value and test to see if imu works with moving angle +#then test without imu to see if actual servo has basic functionality +#then test with imu enabled to see servo will actually move with it +#then hook up to front end and see what clicking lock and unlock mid difference does #Notes: #This scripts takes an input of 0-180 degrees from the test servo publisher node # It uses a math equation to translate this value into a number with a range of 1-12 #The servo specifications indicate a duty cycle of 1-2ms with a total period of 20ms. This would indicate a duty cycle from 5-10 percentage wise. -#However for some reason testing hardcoded values found that it takes 1-12 percent for a full range of 180 degree motion for some reason +#However for some reason testing hardcoded values found that it takes 1-12 value for a full range of 180 degree motion for some reason #that's not mathmatically correct but it works so eh #duty cycle(6) for in the middle. duty cycle(0) is off #for this inclosure the input can be 12 to 120 degrees. the code will automatically make these the max values +#import statements +import RPi.GPIO as GPIO #imports the standard Raspberry Pi GPIO library import rospy +from time import sleep #imports sleep (aka waiting or pause) into the program import message_filters from shared_msgs.msg import servo_msg, imu_msg +MAX_ANGLE = 98 +MIN_ANGLE = 20 + +#Coverts angle to duty cycle, need to update values here +def angleToDuty(angle): + duty = ((angle /180) * 11) + 1 #converts to duty cycle + return duty + +#initialization +GPIO.setmode(GPIO.BCM) #Setting a Pin Mode aka Chose Board pin number scheme +#Set up pin 14 for PWM +GPIO.setup(13, GPIO.OUT) #sets up pin 13 to an output +p = GPIO.PWM(13,50) #sets up pin 13 as a PWM pin(50 is the frequency) +p.start(0) #starts running PWM on the pin and sets it to 0. 0 is the middle +angle_prev = 45 #sets to middle angle +duty_prev = angleToDuty(angle_prev) +lockSet = 0 +imuPitchRef = 0 +p.ChangeDutyCycle(duty_prev) +sleep(0.04) #max time delay +p.ChangeDutyCycle(0) def callback(servoStuff, imuStuff): - print(servoStuff) - print(imuStuff) - #rospy.loginfo(rospy.get_caller_id() + 'I heard %s', servo_cam.data) + #print(imuStuff) + global angle_prev + global duty_prev + global lockSet + global imuPitchRef + imuPitchDiff = 0 + + if(servoStuff.servo_lock_status == True): + if(lockSet == 0): + imuPitchRef = imuStuff.gyro[2] + lockSet = 1 + print(f"lock has been set. imupitchRef = {imuPitchRef}") + imuPitchDiff = imuStuff.gyro[2] - imuPitchRef #might have to switch this to be the other way around + print(f'imuPitchDiff has been found = {imuPitchDiff}') + else: + lockSet = 0 + print("lock has been unset") + + adjustedAngle = servoStuff.angle + imuPitchDiff + if adjustedAngle > MAX_ANGLE: + adjustedAngle = MAX_ANGLE + elif adjustedAngle < MIN_ANGLE: + adjustedAngle = MIN_ANGLE + + if (adjustedAngle != angle_prev): + adjustedDuty = angleToDuty(adjustedAngle) + + dutyDiff = abs(duty_prev - adjustedDuty) + timeToWait = (12.5*(dutyDiff**0.515)+10)/1000 + + + print(f'Old Adjusted angle: {angle_prev}') + print(f'Old Adjusted duty: {duty_prev}') + print(f'Adjusted angle: {adjustedAngle}') + print(f'Adjusted duty: {adjustedDuty}') + print("\n") + p.ChangeDutyCycle(adjustedDuty) + duty_prev = adjustedDuty + angle_prev = adjustedAngle + sleep(timeToWait) #this is so the servo pauses before turning off the power + p.ChangeDutyCycle(0) #if the servo jitters a lot uncomment this it should stop all movement in between calls + + + + + + def listener(): @@ -28,84 +101,18 @@ def listener(): # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. - - servoStuff = message_filters.Subscriber('servo', servo_msg) + print("starting") + servoStuff = message_filters.Subscriber('ServoAngles', servo_msg) imuStuff = message_filters.Subscriber('imu', imu_msg) - combined = message_filters.ApproximateTimeSynchronizer([servoStuff, imuStuff], 10, slop=10) + combined = message_filters.ApproximateTimeSynchronizer([servoStuff, imuStuff], 1, slop=10000) combined.registerCallback(callback) rospy.spin() # spin() simply keeps python from exiting until this node is stopped print("shutting down servo node") - - if __name__ == '__main__': - rospy.init_node('servo_control_node', anonymous=True) + rospy.init_node('listener', anonymous=True) listener() -''' - -import rospy -from std_msgs.msg import Float32 -import RPi.GPIO as GPIO #imports the standard Raspberry Pi GPIO library -import time -from time import sleep #imports sleep (aka waiting or pause) into the program - -#Setting a Pin Mode -GPIO.setmode(GPIO.BCM) #Chose Board pin number scheme -#Set up pin 14 for PWM -GPIO.setup(13, GPIO.OUT) #sets up pin 13 to an output -p = GPIO.PWM(13,50) #sets up pin 13 as a PWM pin(50 is the frequency) -p.start(0) #starts running PWM on the pin and sets it to 0. 0 is the middle -duty_prev = 5.8 #sets to middle duty cycle this is the "middle" -p.ChangeDutyCycle(duty_prev) -sleep(0.04) #max time delay -p.ChangeDutyCycle(0) - -def callback(requestedAngle): - global duty_prev - rate = rospy.Rate(100) - #change the angle to desired duty cycle - duty = ((requestedAngle.data /180) * 11) + 1 #the range of the servo is dutycycle of 1-12 for some reason. So this formula should take in angle of 0-180 and transfer the value from 1-12 - #caps the duty cycle to the max angle values - if duty > 8.02: - duty = 8.02 - elif duty < 2.02: - duty = 2.02 - - dutyDiff = abs(duty_prev - duty) - timeToWait = (12.5*(dutyDiff**0.515)+10)/1000 - - #only run if new number - if (duty != duty_prev): - try: - print(f"Current Duty: {duty:.4f}") - print(f"Prev Duty: {duty_prev:.4f}") - print(f"Duty Diff = {dutyDiff:.6f} and timeToWait = {timeToWait:.6f}\n") - #Debug code for testing a varity of values to test full range of motion - #it = [8.02, 2.02, 8.02, 6.5, 7.5, 6.5, 6.6, 6.5, 2.02] #OKAY so for some reason a duty cycle of 1-12 is used to get a full 180 degrees of rotation. even tho that data sheet indicats 5-10 - #for i in it: - # print(f"Current Duty Cycle: {i}") - # p.ChangeDutyCycle(i) - # time.sleep(2.0) - #print("finished diognostic") - p.ChangeDutyCycle(duty) - duty_prev = duty - except: - print("there has been some error of some type :/") - sleep(timeToWait) #this is so the servo pauses before turning off the power - p.ChangeDutyCycle(0) #if the servo jitters a lot uncomment this it should stop all movement in between calls - - -def listener(): - rospy.init_node('servo_listener_node', anonymous=True) - rospy.Subscriber("servo", Float32, callback) - #rospy.Subscriber("test_servor_angle", Float32, callback) - rospy.spin() #keeps python from exiting until this node is stopped - -if __name__ == '__main__': - #subscribe to the can hardware transmitter - listener() -''' diff --git a/ros/src/i2c/scripts/servo_listener_old.py b/ros/src/i2c/scripts/servo_listener_old.py deleted file mode 100755 index 6adac00..0000000 --- a/ros/src/i2c/scripts/servo_listener_old.py +++ /dev/null @@ -1,73 +0,0 @@ -#! /usr/bin/env python3 - -#Notes: -#This scripts takes an input of 0-180 degrees from the test servo publisher node -# It uses a math equation to translate this value into a number with a range of 1-12 -#The servo specifications indicate a duty cycle of 1-2ms with a total period of 20ms. This would indicate a duty cycle from 5-10 percentage wise. -#However for some reason testing hardcoded values found that it takes 1-12 percent for a full range of 180 degree motion for some reason -#that's not mathmatically correct but it works so eh -#duty cycle(6) for in the middle. duty cycle(0) is off -#for this inclosure the input can be 12 to 120 degrees. the code will automatically make these the max values - -import rospy -from std_msgs.msg import Float32 -import RPi.GPIO as GPIO #imports the standard Raspberry Pi GPIO library -import time -from time import sleep #imports sleep (aka waiting or pause) into the program - -#Setting a Pin Mode -GPIO.setmode(GPIO.BCM) #Chose Board pin number scheme -#Set up pin 14 for PWM -GPIO.setup(13, GPIO.OUT) #sets up pin 13 to an output -p = GPIO.PWM(13,50) #sets up pin 13 as a PWM pin(50 is the frequency) -p.start(0) #starts running PWM on the pin and sets it to 0. 0 is the middle -duty_prev = 5.8 #sets to middle duty cycle this is the "middle" -p.ChangeDutyCycle(duty_prev) -sleep(0.04) #max time delay -p.ChangeDutyCycle(0) - -def callback(requestedAngle): - global duty_prev - rate = rospy.Rate(100) - #change the angle to desired duty cycle - duty = ((requestedAngle.data /180) * 11) + 1 #the range of the servo is dutycycle of 1-12 for some reason. So this formula should take in angle of 0-180 and transfer the value from 1-12 - #caps the duty cycle to the max angle values - if duty > 8.02: - duty = 8.02 - elif duty < 2.02: - duty = 2.02 - - dutyDiff = abs(duty_prev - duty) - timeToWait = (12.5*(dutyDiff**0.515)+10)/1000 - - #only run if new number - if (duty != duty_prev): - try: - print(f"Current Duty: {duty:.4f}") - print(f"Prev Duty: {duty_prev:.4f}") - print(f"Duty Diff = {dutyDiff:.6f} and timeToWait = {timeToWait:.6f}\n") - #Debug code for testing a varity of values to test full range of motion - #it = [8.02, 2.02, 8.02, 6.5, 7.5, 6.5, 6.6, 6.5, 2.02] #OKAY so for some reason a duty cycle of 1-12 is used to get a full 180 degrees of rotation. even tho that data sheet indicats 5-10 - #for i in it: - # print(f"Current Duty Cycle: {i}") - # p.ChangeDutyCycle(i) - # time.sleep(2.0) - #print("finished diognostic") - p.ChangeDutyCycle(duty) - duty_prev = duty - except: - print("there has been some error of some type :/") - sleep(timeToWait) #this is so the servo pauses before turning off the power - p.ChangeDutyCycle(0) #if the servo jitters a lot uncomment this it should stop all movement in between calls - - -def listener(): - rospy.init_node('servo_listener_node', anonymous=True) - rospy.Subscriber("servo", Float32, callback) - #rospy.Subscriber("test_servor_angle", Float32, callback) - rospy.spin() #keeps python from exiting until this node is stopped - -if __name__ == '__main__': - #subscribe to the can hardware transmitter - listener() - diff --git a/ros/src/i2c/scripts/servo_talker.py b/ros/src/i2c/scripts/servo_talker.py index 4ca133f..ad05399 100755 --- a/ros/src/i2c/scripts/servo_talker.py +++ b/ros/src/i2c/scripts/servo_talker.py @@ -1,37 +1,24 @@ -#! /usr/bin/env python3 - +#! /usr/bin/python3 import rospy import time from std_msgs.msg import Float32 from shared_msgs.msg import servo_msg def talker(): - rospy.init_node('servo_debug_talker_node', anonymous=True) - pub = rospy.Publisher('servo', servo_msg, queue_size=10) - rate = rospy.Rate(10) #10Hz - i = 0 - + rospy.init_node('other_talker', anonymous=True) + pub = rospy.Publisher('ServoAngles', servo_msg, queue_size=10) + rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): - servoCam = servo_msg() - servoCam.angle = float(input("Insert desired angle: ")) - servoCam.servo_lock_status = False #might have to be false - servoCam.header.stamp = rospy.Time.now() #also might have to include - servoCam.header.frame_id = "/ServoTest" #might have to include - - pub.publish(servoCam) - - #if i%2 == 0: - # angle = 0.0 - # print(angle) - # pub.publish(angle) - #else: - # angle = 180.0 - # print(angle) - # pub.publish(angle) - #i += 1 - - #time.sleep(4) + servoCam.angle = float(input("Insert desired angle: ")) + servoCam.servo_lock_status = False + servoCam.header.stamp = rospy.Time.now() + servoCam.header.frame_id = "/ServoTest" + + print(servoCam) + rospy.loginfo(servoCam) + pub.publish(servoCam) + rate.sleep() if __name__ == '__main__': try: diff --git a/ros/src/i2c_com/README.txt b/ros/src/i2c_com/README.txt deleted file mode 100644 index 0c55f2a..0000000 --- a/ros/src/i2c_com/README.txt +++ /dev/null @@ -1,9 +0,0 @@ -Welcome to i2c_comm, the i2c communication package. - -This package handles communication to the i2c hardware. This package -does not do any processing and merely relays data onto ROS. - -Nodes: - * i2c_comm: - -subs: i2c_tx - -pubs: i2c_rx diff --git a/ros/src/i2c_com/package.xml b/ros/src/i2c_com/package.xml deleted file mode 100644 index 19236cb..0000000 --- a/ros/src/i2c_com/package.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - i2c_com - 0.0.0 - The i2c_com package - TODO - - TODO - - catkin - shared_msgs - shared_msgs - - diff --git a/ros/src/i2c_com/scripts/BNO055.py b/ros/src/i2c_com/scripts/BNO055.py deleted file mode 100644 index b814f18..0000000 --- a/ros/src/i2c_com/scripts/BNO055.py +++ /dev/null @@ -1,184 +0,0 @@ -from Adafruit_BNO055 import BNO055 as _BNO055 - - -class BNO055(object): - def __init__(self): - # IMU Reset Pin connected to Pin 18 - self._bno = _BNO055.BNO055(rst=18) - - # Fail if it cannot be initialized - if not self._bno.begin(): - raise I2CERROR('Failed to initialize BNO055!') - - self._data = { - 'euler': { - # Resolution found from a forumn post - 'yaw': 0, # Rotation about z axis (vertical) +/- 0.01 degree - 'roll': 0, # Rotation about y axix (perpindicular to the pins IMU) +/- 0.01 degree - 'pitch': 0, # Rotation about x axis (parallel to the pins of IMU) +/- 0.01 degree - - }, - 'gyro': { - 'x': 0, # 3e-2 degree/sec - 'y': 0, # 3e-2 degree/sec - 'z': 0, # 3e-2 degree/sec - }, - 'acceleration': { - 'x': 0, # +/- 5e-4 g - 'y': 0, # +/- 5e-4 g - 'z': 0, # +/- 5e-4 g - }, - 'linear_acceleration': { - 'x': 0, # +/- 0.25 m/s^2 - 'y': 0, # +/- 0.25 m/s^2 - 'z': 0, # +/- 0.25 m/s^2 - }, - 'temp': 0, # Good enough - } - - @property - def data(self): - return self._data - - def roll(self): - return self._data['euler']['roll'] - - def pitch(self): - return self._data['euler']['pitch'] - - def yaw(self): - return self._data['euler']['yaw'] - - def gyro_x(self): - return self._data['gyro']['x'] - - def gyro_y(self): - return self._data['gyro']['y'] - - def gyro_z(self): - return self._data['gyro']['z'] - - def acceleration_x(self): - return self._data['acceleration']['x'] - - def acceleration_y(self): - return self._data['acceleration']['y'] - - def acceleration_z(self): - return self._data['acceleration']['z'] - - def linear_acceleration_x(self): - return self._data['linear_acceleration']['x'] - - def linear_acceleration_y(self): - return self._data['linear_acceleration']['y'] - - def linear_acceleration_z(self): - return self._data['linear_acceleration']['z'] - - def update(self): - euler = self._bno.read_euler() - self._data['euler']['yaw'] = euler[0] - self._data['euler']['roll'] = euler[1] - self._data['euler']['pitch'] = euler[2] - - gyro = self._bno.read_gyroscope() - self._data['gyro']['x'] = gyro[0] - self._data['gyro']['y'] = gyro[1] - self._data['gyro']['z'] = gyro[2] - - acceleration = self._bno.read_accelerometer() - self._data['acceleration']['x'] = acceleration[0] - self._data['acceleration']['y'] = acceleration[1] - self._data['acceleration']['z'] = acceleration[2] - - linear_accel = self._bno.read_linear_acceleration() - self._data['linear_acceleration']['x'] = linear_accel[0] - self._data['linear_acceleration']['y'] = linear_accel[1] - self._data['linear_acceleration']['z'] = linear_accel[2] - - temp = self._bno.read_temp() - self._data['temp'] = temp - - return True - - def get_calibration(self): - return self._bno.get_calibration() - - def reset_calibration(self): - cal_array_original = self.get_calibration() - self._bno.set_calibration(cal_array_original); - return cal_array_original - - def set_calibration(self, data): - self._bno.set_calibration(data) - - def sitrep(self): - sys, gyro, accel, mag = self._bno.get_calibration_status() - sys_stat, sys_test, sys_err = self._bno.get_system_status(True) - good_status = [3, 3, 3, 3, 1, 0x0F, 0] - test_array = [sys, gyro, accel, mag, sys_stat, sys_test, sys_err] - - for x in range(0, 4): - if test_array[x] != 3: - return False - - if test_array[4] == 1: - return False - - if test_array[5] != 0x0F: - return False - - if test_array[6] != 0: - return False - - return True - - -if __name__ == '__main__': - # BNO055().main() - import time - - - def main(): - sensor = BNO055() # Default I2C bus is 1 (Raspberry Pi 3) - - # We must initialize the sensor before reading it - if not sensor: - print "Sensor could not be initialized" - exit(1) - - # print("Pressure: %.2f mbar") % (sensor.pressure()) - - # print("Temperature: %.2f C") % (sensor.temperature(ms5837.UNITS_Centigrade)) - - # time.sleep(2) - - print("Time \tRoll \tPitch \tYaw \tGyro: \tx \ty \tz \tACC: \tx \ty \tz \tLinear: \tx \ty \tz") - - # Spew readings - while True: - if sensor.update(): - print( - "%s \t%0.2f \t%0.2f \t%0.2f \t\t%0.2f \t%0.2f \t%0.2f \t\t%0.2f \t%0.2f \t%0.2f \t\t%0.2f \t%0.2f \t%0.2f") % ( - time.strftime("%H:%M:%S", time.localtime()) + '.%d' % (time.time() % 1 * 1000), - sensor.roll(), - sensor.pitch(), - sensor.yaw(), - sensor.gyro_x(), - sensor.gyro_y(), - sensor.gyro_z(), - sensor.acceleration_x(), - sensor.acceleration_y(), - sensor.acceleration_z(), - sensor.linear_acceleration_x(), - sensor.linear_acceleration_y(), - sensor.linear_acceleration_z()) - - time.sleep(0.005) - else: - print "Sensor read failed!" - exit(1) - - - main() diff --git a/ros/src/i2c_com/scripts/BNO055.pyc b/ros/src/i2c_com/scripts/BNO055.pyc deleted file mode 100644 index f86b3b0..0000000 Binary files a/ros/src/i2c_com/scripts/BNO055.pyc and /dev/null differ diff --git a/ros/src/i2c_com/scripts/TYS01.py b/ros/src/i2c_com/scripts/TYS01.py deleted file mode 100644 index bc90616..0000000 --- a/ros/src/i2c_com/scripts/TYS01.py +++ /dev/null @@ -1,84 +0,0 @@ -import smbus -from time import sleep - -# Valid units -UNITS_Centigrade = 1 -UNITS_Farenheit = 2 -UNITS_Kelvin = 3 - - -class TSYS01(object): - # Registers - _TSYS01_ADDR = 0x77 - _TSYS01_PROM_READ = 0xA0 - _TSYS01_RESET = 0x1E - _TSYS01_CONVERT = 0x48 - _TSYS01_READ = 0x00 - - def __init__(self, bus=1): - # Degrees C - self._temperature = 0 - self._k = [] - - try: - self._bus = smbus.SMBus(bus) - except: - print("Bus %d is not available.") % bus - print("Available busses are listed as /dev/i2c*") - self._bus = None - raise I2CERROR('failed to start TYS01') - - def init(self): - if self._bus is None: - "No bus!" - return False - - self._bus.write_byte(self._TSYS01_ADDR, self._TSYS01_RESET) - - # Wait for reset to complete - sleep(0.1) - - self._k = [] - - # Read calibration values - # Read one 16 bit byte word at a time - for prom in range(0xAA, 0xA2 - 2, -2): - k = self._bus.read_word_data(self._TSYS01_ADDR, prom) - k = ((k & 0xFF) << 8) | (k >> 8) # SMBus is little-endian for word transfers, we need to swap MSB and LSB - self._k.append(k) - - return True - - def read(self): - if self._bus is None: - print "No bus!" - return False - - # Request conversion - self._bus.write_byte(self._TSYS01_ADDR, self._TSYS01_CONVERT) - - # Max conversion time = 9.04 ms - sleep(0.01) - - adc = self._bus.read_i2c_block_data(self._TSYS01_ADDR, self._TSYS01_READ, 3) - adc = adc[0] << 16 | adc[1] << 8 | adc[2] - self._calculate(adc) - return True - - # Temperature in requested units - # default degrees C - def temperature(self, conversion=UNITS_Centigrade): - if conversion == UNITS_Farenheit: - return (9 / 5) * self._temperature + 32 - elif conversion == UNITS_Kelvin: - return self._temperature - 273 - return self._temperature - - # Cribbed from datasheet - def _calculate(self, adc): - adc16 = adc / 256 - self._temperature = -2 * self._k[4] * 10 ** -21 * adc16 ** 4 + \ - 4 * self._k[3] * 10 ** -16 * adc16 ** 3 + \ - -2 * self._k[2] * 10 ** -11 * adc16 ** 2 + \ - 1 * self._k[1] * 10 ** -6 * adc16 + \ - -1.5 * self._k[0] * 10 ** -2 diff --git a/ros/src/i2c_com/scripts/TYS01.pyc b/ros/src/i2c_com/scripts/TYS01.pyc deleted file mode 100644 index 447cc00..0000000 Binary files a/ros/src/i2c_com/scripts/TYS01.pyc and /dev/null differ diff --git a/ros/src/i2c_com/scripts/i2c.py b/ros/src/i2c_com/scripts/i2c.py deleted file mode 100755 index f320af9..0000000 --- a/ros/src/i2c_com/scripts/i2c.py +++ /dev/null @@ -1,54 +0,0 @@ -#! /usr/bin/python -import rospy -import time -import smbus -from BNO055 import BNO055 -from TYS01 import TSYS01 -from ms5837 import MS5837 -from shared_msgs.msg import imu_msg, temp_msg, depth_msg - -IMU_PUB_RATE = 50 - - -def message_received(msg): - # This runs on a seperate thread from the pub - pass - - -if __name__ == "__main__": - rospy.init_node('i2c_node') - #imu_pub = rospy.Publisher('imu_data', imu_msg, queue_size=1) - rate = rospy.Rate(IMU_PUB_RATE) - temp_pub = rospy.Publisher('temp_data', temp_msg, queue_size=1) - depth_pub = rospy.Publisher('depth_data', depth_msg, queue_size=1) - depth_sens = MS5837() - try: - #temp_sens = TSYS01() - pass - #temp_sens.init() - except: - # add mock classes to return 0 and continue and alert pilots - print('expected sensor not found') - pass - while not rospy.is_shutdown(): - # using imu rate to poll the temp sensor as well beacuse IMU updates faster - - - # out_message = imu_msg() - # out_message.gyro = [imu_sens.pitch(), imu_sens.yaw(), imu_sens.roll()] - # out_message.accel = [imu_sens.acceleration_x(), imu_sens.acceleration_y(), imu_sens.acceleration_z()] - # imu_pub.publish(out_message) - - # read the temp in C and send it to a ros - - # temp_message = temp_msg() - # temp_message.temperature = {temp_sens.temperature()} - # temp_pub.publish(temp_message) - - # depth sensor updating - - depth_sens.update() - depth_message = depth_msg() - depth_message.pressure = depth_sens.depth() - depth_pub.publish(depth_message) - rate.sleep() diff --git a/ros/src/i2c_com/scripts/ms5837.py b/ros/src/i2c_com/scripts/ms5837.py deleted file mode 100644 index 500e1f3..0000000 --- a/ros/src/i2c_com/scripts/ms5837.py +++ /dev/null @@ -1,307 +0,0 @@ -try: - import smbus -except: - print 'Try sudo apt-get install python-smbus' - -import time - -# Models -MODEL_02BA = 0 -MODEL_30BA = 1 - -# Oversampling options -OSR_256 = 0 -OSR_512 = 1 -OSR_1024 = 2 -OSR_2048 = 3 -OSR_4096 = 4 -OSR_8192 = 5 - -# kg/m^3 convenience -DENSITY_FRESHWATER = 997 -DENSITY_SALTWATER = 1029 - -# Conversion factors (from native unit, mbar) -UNITS_Pa = 100.0 -UNITS_hPa = 1.0 -UNITS_kPa = 0.1 -UNITS_mbar = 1.0 -UNITS_bar = 0.001 -UNITS_atm = 0.000986923 -UNITS_Torr = 0.750062 -UNITS_psi = 0.014503773773022 - -# Valid units -UNITS_Centigrade = 1 -UNITS_Farenheit = 2 -UNITS_Kelvin = 3 - - -class MS5837(object): - # Registers - _MS5837_ADDR = 0x76 - _MS5837_RESET = 0x1E - _MS5837_ADC_READ = 0x00 - _MS5837_PROM_READ = 0xA0 - _MS5837_CONVERT_D1_256 = 0x40 - _MS5837_CONVERT_D2_256 = 0x50 - - def __init__(self, model=MODEL_30BA, bus=1): - self._model = model - - try: - self._bus = smbus.SMBus(bus) - except: - print("Bus %d is not available.") % bus - print("Available busses are listed as /dev/i2c*") - self._bus = None - raise I2CERROR('failed to start MS5827') - - self._fluidDensity = DENSITY_FRESHWATER - self._pressure = 0 - self._temperature = 0 - self._D1 = 0 - self._D2 = 0 - - if self._bus is None: - "No bus!" - return - - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_RESET) - - # Wait for reset to complete - time.sleep(0.01) - - self._C = [] - - # Read calibration values and CRC - for i in range(7): - c = self._bus.read_word_data(self._MS5837_ADDR, self._MS5837_PROM_READ + 2 * i) - c = ((c & 0xFF) << 8) | (c >> 8) # SMBus is little-endian for word transfers, we need to swap MSB and LSB - self._C.append(c) - - crc = (self._C[0] & 0xF000) >> 12 - if crc != self._crc4(self._C): - print "PROM read error, CRC failed!" - return - - # for pressure calibration for depth - PressDepthConv = 0.01 - - self._data = { - "pressure": 0, # +/- 50 milibars - "temperature": 0 # +/- 1.5 celsius - } - - # for dynamicDepth pressure calibration - self.initPressure = 0 - self.conv = PressDepthConv - self.update() - self.initPressure = self._data['pressure'] - - def read(self, oversampling=OSR_8192): - if self._bus is None: - print "No bus!" - return False - - if oversampling < OSR_256 or oversampling > OSR_8192: - print "Invalid oversampling option!" - return False - - # Request D1 conversion (temperature) - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D1_256 + 2 * oversampling) - - # Maximum conversion time increases linearly with oversampling - # max time (seconds) ~= 2.2e-6(x) where x = OSR = (2^8, 2^9, ..., 2^13) - # We use 2.5e-6 for some overhead - time.sleep(2.5e-6 * 2 ** (8 + oversampling)) - - d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) - self._D1 = d[0] << 16 | d[1] << 8 | d[2] - - # Request D2 conversion (pressure) - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D2_256 + 2 * oversampling) - - # As above - time.sleep(2.5e-6 * 2 ** (8 + oversampling)) - - d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) - self._D2 = d[0] << 16 | d[1] << 8 | d[2] - - # Calculate compensated pressure and temperature - # using raw ADC values and internal calibration - self._calculate() - - return True - - def setFluidDensity(self, denisty): - self._fluidDensity = denisty - - # Pressure in requested units - # mbar * conversion - def pressure(self, conversion=UNITS_mbar): - return self._pressure * conversion - - # Temperature in requested units - # default degrees C - def temperature(self, conversion=UNITS_Centigrade): - degC = self._temperature / 100.0 - if conversion == UNITS_Farenheit: - return (9 / 5) * degC + 32 - elif conversion == UNITS_Kelvin: - return degC - 273 - return degC - - # Depth relative to MSL pressure in given fluid density - def depth(self): - return (self.pressure(UNITS_Pa) - 101300) / (self._fluidDensity * 9.80665) - - # Depth relative to surface pressure - @property - def data(self): - # self._data['depth'] = self._data['pressure']*self.conv - # return self._data - self._data['pressure'] = self.pressure() - self._data['depth'] = self.depth() - return self._data - - # update pressure and temperature - def update(self): - if self.read(): - # pressure in mBars - pressure = self.pressure() - self.initPressure - self._data['pressure'] = pressure - - # temp in celsius - temperature = self.temperature() - self._data['temperature'] = temperature - else: - pass - - # Altitude relative to MSL pressure - def altitude(self): - return (1 - pow((self.pressure() / 1013.25), .190284)) * 145366.45 * .3048 - - # Cribbed from datasheet - def _calculate(self): - OFFi = 0 - SENSi = 0 - Ti = 0 - - dT = self._D2 - self._C[5] * 256 - if self._model == MODEL_02BA: - SENS = self._C[1] * 65536 + (self._C[3] * dT) / 128 - OFF = self._C[2] * 131072 + (self._C[4] * dT) / 64 - self._pressure = (self._D1 * SENS / (2097152) - OFF) / (32768) - else: - SENS = self._C[1] * 32768 + (self._C[3] * dT) / 256 - OFF = self._C[2] * 65536 + (self._C[4] * dT) / 128 - self._pressure = (self._D1 * SENS / (2097152) - OFF) / (8192) - - self._temperature = 2000 + dT * self._C[6] / 8388608 - - # Second order compensation - if self._model == MODEL_02BA: - if (self._temperature / 100) < 20: # Low temp - Ti = (11 * dT * dT) / (34359738368) - OFFi = (31 * (self._temperature - 2000) * (self._temperature - 2000)) / 8 - SENSi = (63 * (self._temperature - 2000) * (self._temperature - 2000)) / 32 - - else: - if (self._temperature / 100) < 20: # Low temp - Ti = (3 * dT * dT) / (8589934592) - OFFi = (3 * (self._temperature - 2000) * (self._temperature - 2000)) / 2 - SENSi = (5 * (self._temperature - 2000) * (self._temperature - 2000)) / 8 - if (self._temperature / 100) < -15: # Very low temp - OFFi = OFFi + 7 * (self._temperature + 1500l) * (self._temperature + 1500) - SENSi = SENSi + 4 * (self._temperature + 1500l) * (self._temperature + 1500) - elif (self._temperature / 100) >= 20: # High temp - Ti = 2 * (dT * dT) / (137438953472) - OFFi = (1 * (self._temperature - 2000) * (self._temperature - 2000)) / 16 - SENSi = 0 - - OFF2 = OFF - OFFi - SENS2 = SENS - SENSi - - if self._model == MODEL_02BA: - self._temperature = (self._temperature - Ti) - self._pressure = (((self._D1 * SENS2) / 2097152 - OFF2) / 32768) / 100.0 - else: - self._temperature = (self._temperature - Ti) - self._pressure = (((self._D1 * SENS2) / 2097152 - OFF2) / 8192) / 10.0 - - # Cribbed from datasheet - def _crc4(self, n_prom): - n_rem = 0 - - n_prom[0] = ((n_prom[0]) & 0x0FFF) - n_prom.append(0) - - for i in range(16): - if i % 2 == 1: - n_rem ^= ((n_prom[i >> 1]) & 0x00FF) - else: - n_rem ^= (n_prom[i >> 1] >> 8) - - for n_bit in range(8, 0, -1): - if n_rem & 0x8000: - n_rem = (n_rem << 1) ^ 0x3000 - else: - n_rem = (n_rem << 1) - - n_rem = ((n_rem >> 12) & 0x000F) - - self.n_prom = n_prom - self.n_rem = n_rem - - return n_rem ^ 0x00 - - -class MS5837_30BA(MS5837): - def __init__(self, bus=1): - MS5837.__init__(self, MODEL_30BA, bus) - - -class MS5837_02BA(MS5837): - def __init__(self, bus=1): - MS5837.__init__(self, MODEL_02BA, bus) - - -if __name__ == '__main__': - import time - - - def main(): - sensor = MS5837() # Default I2C bus is 1 (Raspberry Pi 3) - - # We must initialize the sensor before reading it - # if not sensor.init(): - # print "Sensor could not be initialized" - # exit(1) - - # We have to read values from sensor to update pressure and temperature - if not sensor.read(): - print "Sensor read failed!" - exit(1) - - # print("Pressure: %.2f mbar") % (sensor.pressure()) - - # print("Temperature: %.2f C") % (sensor.temperature(ms5837.UNITS_Centigrade)) - - # time.sleep(2) - - print("Time \tPressure (mbar) \tTemperature (C)") - - # Spew readings - while True: - if sensor.read(): - print("%s \t%0.1f \t%0.2f") % ( - time.strftime("%H:%M:%S", time.localtime()) + '.%d' % (time.time() % 1 * 1000), - sensor.pressure(), # Default is mbar (no arguments) - sensor.depth()) # Default is degrees C (no arguments) - else: - print "Sensor read failed!" - exit(1) - - - main() diff --git a/ros/src/i2c_com/scripts/ms5837.pyc b/ros/src/i2c_com/scripts/ms5837.pyc deleted file mode 100644 index 5f4fdd1..0000000 Binary files a/ros/src/i2c_com/scripts/ms5837.pyc and /dev/null differ diff --git a/ros/src/i2c_com/scripts/testdummy.py b/ros/src/i2c_com/scripts/testdummy.py deleted file mode 100644 index 8546ee7..0000000 --- a/ros/src/i2c_com/scripts/testdummy.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python - -import rospy -from geometry_msgs.msg import Twist -from std_msgs.msg import Float32 - -def talker(): - rospy.init_node('test_dummy', anonymous=True) - pub1 = rospy.Publisher('/surface_imu', Twist, queue_size=10) - pub2 = rospy.Publisher('/surface_depth', Float32, queue_size=10) - rate = rospy.Rate(50) - i = 0 - while not rospy.is_shutdown(): - msg = Twist() - meh = Float32() - meh = 12.34 - msg.linear.x = i - msg.linear.y = 1 - msg.linear.z = -1 - msg.angular.x = 0 - msg.angular.y = 1 - msg.angular.z = -1 - pub1.publish(msg) - pub2.publish(meh) - i += 1 - rate.sleep() - -if __name__ == '__main__': - talker() \ No newline at end of file diff --git a/ros/src/i2c_proc/CMakeLists.txt b/ros/src/i2c_proc/CMakeLists.txt deleted file mode 100644 index b2f9146..0000000 --- a/ros/src/i2c_proc/CMakeLists.txt +++ /dev/null @@ -1,198 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(i2c_proc) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - shared_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# shared_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES i2c_proc -# CATKIN_DEPENDS rospy shared_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/i2c_proc.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/i2c_proc_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_i2c_proc.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/ros/src/i2c_proc/package.xml b/ros/src/i2c_proc/package.xml deleted file mode 100644 index 17c0ebe..0000000 --- a/ros/src/i2c_proc/package.xml +++ /dev/null @@ -1,65 +0,0 @@ - - - i2c_proc - 0.0.0 - The i2c_proc package - - - - - keith - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - shared_msgs - rospy - shared_msgs - rospy - shared_msgs - - - - - - - - diff --git a/ros/src/i2c_proc/scripts/BNO055.py b/ros/src/i2c_proc/scripts/BNO055.py deleted file mode 100644 index b814f18..0000000 --- a/ros/src/i2c_proc/scripts/BNO055.py +++ /dev/null @@ -1,184 +0,0 @@ -from Adafruit_BNO055 import BNO055 as _BNO055 - - -class BNO055(object): - def __init__(self): - # IMU Reset Pin connected to Pin 18 - self._bno = _BNO055.BNO055(rst=18) - - # Fail if it cannot be initialized - if not self._bno.begin(): - raise I2CERROR('Failed to initialize BNO055!') - - self._data = { - 'euler': { - # Resolution found from a forumn post - 'yaw': 0, # Rotation about z axis (vertical) +/- 0.01 degree - 'roll': 0, # Rotation about y axix (perpindicular to the pins IMU) +/- 0.01 degree - 'pitch': 0, # Rotation about x axis (parallel to the pins of IMU) +/- 0.01 degree - - }, - 'gyro': { - 'x': 0, # 3e-2 degree/sec - 'y': 0, # 3e-2 degree/sec - 'z': 0, # 3e-2 degree/sec - }, - 'acceleration': { - 'x': 0, # +/- 5e-4 g - 'y': 0, # +/- 5e-4 g - 'z': 0, # +/- 5e-4 g - }, - 'linear_acceleration': { - 'x': 0, # +/- 0.25 m/s^2 - 'y': 0, # +/- 0.25 m/s^2 - 'z': 0, # +/- 0.25 m/s^2 - }, - 'temp': 0, # Good enough - } - - @property - def data(self): - return self._data - - def roll(self): - return self._data['euler']['roll'] - - def pitch(self): - return self._data['euler']['pitch'] - - def yaw(self): - return self._data['euler']['yaw'] - - def gyro_x(self): - return self._data['gyro']['x'] - - def gyro_y(self): - return self._data['gyro']['y'] - - def gyro_z(self): - return self._data['gyro']['z'] - - def acceleration_x(self): - return self._data['acceleration']['x'] - - def acceleration_y(self): - return self._data['acceleration']['y'] - - def acceleration_z(self): - return self._data['acceleration']['z'] - - def linear_acceleration_x(self): - return self._data['linear_acceleration']['x'] - - def linear_acceleration_y(self): - return self._data['linear_acceleration']['y'] - - def linear_acceleration_z(self): - return self._data['linear_acceleration']['z'] - - def update(self): - euler = self._bno.read_euler() - self._data['euler']['yaw'] = euler[0] - self._data['euler']['roll'] = euler[1] - self._data['euler']['pitch'] = euler[2] - - gyro = self._bno.read_gyroscope() - self._data['gyro']['x'] = gyro[0] - self._data['gyro']['y'] = gyro[1] - self._data['gyro']['z'] = gyro[2] - - acceleration = self._bno.read_accelerometer() - self._data['acceleration']['x'] = acceleration[0] - self._data['acceleration']['y'] = acceleration[1] - self._data['acceleration']['z'] = acceleration[2] - - linear_accel = self._bno.read_linear_acceleration() - self._data['linear_acceleration']['x'] = linear_accel[0] - self._data['linear_acceleration']['y'] = linear_accel[1] - self._data['linear_acceleration']['z'] = linear_accel[2] - - temp = self._bno.read_temp() - self._data['temp'] = temp - - return True - - def get_calibration(self): - return self._bno.get_calibration() - - def reset_calibration(self): - cal_array_original = self.get_calibration() - self._bno.set_calibration(cal_array_original); - return cal_array_original - - def set_calibration(self, data): - self._bno.set_calibration(data) - - def sitrep(self): - sys, gyro, accel, mag = self._bno.get_calibration_status() - sys_stat, sys_test, sys_err = self._bno.get_system_status(True) - good_status = [3, 3, 3, 3, 1, 0x0F, 0] - test_array = [sys, gyro, accel, mag, sys_stat, sys_test, sys_err] - - for x in range(0, 4): - if test_array[x] != 3: - return False - - if test_array[4] == 1: - return False - - if test_array[5] != 0x0F: - return False - - if test_array[6] != 0: - return False - - return True - - -if __name__ == '__main__': - # BNO055().main() - import time - - - def main(): - sensor = BNO055() # Default I2C bus is 1 (Raspberry Pi 3) - - # We must initialize the sensor before reading it - if not sensor: - print "Sensor could not be initialized" - exit(1) - - # print("Pressure: %.2f mbar") % (sensor.pressure()) - - # print("Temperature: %.2f C") % (sensor.temperature(ms5837.UNITS_Centigrade)) - - # time.sleep(2) - - print("Time \tRoll \tPitch \tYaw \tGyro: \tx \ty \tz \tACC: \tx \ty \tz \tLinear: \tx \ty \tz") - - # Spew readings - while True: - if sensor.update(): - print( - "%s \t%0.2f \t%0.2f \t%0.2f \t\t%0.2f \t%0.2f \t%0.2f \t\t%0.2f \t%0.2f \t%0.2f \t\t%0.2f \t%0.2f \t%0.2f") % ( - time.strftime("%H:%M:%S", time.localtime()) + '.%d' % (time.time() % 1 * 1000), - sensor.roll(), - sensor.pitch(), - sensor.yaw(), - sensor.gyro_x(), - sensor.gyro_y(), - sensor.gyro_z(), - sensor.acceleration_x(), - sensor.acceleration_y(), - sensor.acceleration_z(), - sensor.linear_acceleration_x(), - sensor.linear_acceleration_y(), - sensor.linear_acceleration_z()) - - time.sleep(0.005) - else: - print "Sensor read failed!" - exit(1) - - - main() diff --git a/ros/src/i2c_proc/scripts/BNO055.pyc b/ros/src/i2c_proc/scripts/BNO055.pyc deleted file mode 100644 index 0271b83..0000000 Binary files a/ros/src/i2c_proc/scripts/BNO055.pyc and /dev/null differ diff --git a/ros/src/i2c_proc/scripts/BNO055new.py b/ros/src/i2c_proc/scripts/BNO055new.py deleted file mode 100644 index 91dafc3..0000000 --- a/ros/src/i2c_proc/scripts/BNO055new.py +++ /dev/null @@ -1,689 +0,0 @@ -# Adafruit BNO055 Absolute Orientation Sensor Library -# Copyright (c) 2015 Adafruit Industries -# Author: Tony DiCola -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -import binascii -import logging -import struct -import time - -import serial - - -# I2C addresses -BNO055_ADDRESS_A = 0x28 -BNO055_ADDRESS_B = 0x29 -BNO055_ID = 0xA0 - -# Page id register definition -BNO055_PAGE_ID_ADDR = 0X07 - -# PAGE0 REGISTER DEFINITION START -BNO055_CHIP_ID_ADDR = 0x00 -BNO055_ACCEL_REV_ID_ADDR = 0x01 -BNO055_MAG_REV_ID_ADDR = 0x02 -BNO055_GYRO_REV_ID_ADDR = 0x03 -BNO055_SW_REV_ID_LSB_ADDR = 0x04 -BNO055_SW_REV_ID_MSB_ADDR = 0x05 -BNO055_BL_REV_ID_ADDR = 0X06 - -# Accel data register -BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08 -BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09 -BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A -BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B -BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C -BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D - -# Mag data register -BNO055_MAG_DATA_X_LSB_ADDR = 0X0E -BNO055_MAG_DATA_X_MSB_ADDR = 0X0F -BNO055_MAG_DATA_Y_LSB_ADDR = 0X10 -BNO055_MAG_DATA_Y_MSB_ADDR = 0X11 -BNO055_MAG_DATA_Z_LSB_ADDR = 0X12 -BNO055_MAG_DATA_Z_MSB_ADDR = 0X13 - -# Gyro data registers -BNO055_GYRO_DATA_X_LSB_ADDR = 0X14 -BNO055_GYRO_DATA_X_MSB_ADDR = 0X15 -BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16 -BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17 -BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18 -BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19 - -# Euler data registers -BNO055_EULER_H_LSB_ADDR = 0X1A -BNO055_EULER_H_MSB_ADDR = 0X1B -BNO055_EULER_R_LSB_ADDR = 0X1C -BNO055_EULER_R_MSB_ADDR = 0X1D -BNO055_EULER_P_LSB_ADDR = 0X1E -BNO055_EULER_P_MSB_ADDR = 0X1F - -# Quaternion data registers -BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20 -BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21 -BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22 -BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23 -BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24 -BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25 -BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26 -BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27 - -# Linear acceleration data registers -BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28 -BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29 -BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A -BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B -BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C -BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D - -# Gravity data registers -BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E -BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F -BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30 -BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31 -BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32 -BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33 - -# Temperature data register -BNO055_TEMP_ADDR = 0X34 - -# Status registers -BNO055_CALIB_STAT_ADDR = 0X35 -BNO055_SELFTEST_RESULT_ADDR = 0X36 -BNO055_INTR_STAT_ADDR = 0X37 - -BNO055_SYS_CLK_STAT_ADDR = 0X38 -BNO055_SYS_STAT_ADDR = 0X39 -BNO055_SYS_ERR_ADDR = 0X3A - -# Unit selection register -BNO055_UNIT_SEL_ADDR = 0X3B -BNO055_DATA_SELECT_ADDR = 0X3C - -# Mode registers -BNO055_OPR_MODE_ADDR = 0X3D -BNO055_PWR_MODE_ADDR = 0X3E - -BNO055_SYS_TRIGGER_ADDR = 0X3F -BNO055_TEMP_SOURCE_ADDR = 0X40 - -# Axis remap registers -BNO055_AXIS_MAP_CONFIG_ADDR = 0X41 -BNO055_AXIS_MAP_SIGN_ADDR = 0X42 - -# Axis remap values -AXIS_REMAP_X = 0x00 -AXIS_REMAP_Y = 0x01 -AXIS_REMAP_Z = 0x02 -AXIS_REMAP_POSITIVE = 0x00 -AXIS_REMAP_NEGATIVE = 0x01 - -# SIC registers -BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43 -BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44 -BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45 -BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46 -BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47 -BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48 -BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49 -BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A -BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B -BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C -BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D -BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E -BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F -BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50 -BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51 -BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52 -BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53 -BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54 - -# Accelerometer Offset registers -ACCEL_OFFSET_X_LSB_ADDR = 0X55 -ACCEL_OFFSET_X_MSB_ADDR = 0X56 -ACCEL_OFFSET_Y_LSB_ADDR = 0X57 -ACCEL_OFFSET_Y_MSB_ADDR = 0X58 -ACCEL_OFFSET_Z_LSB_ADDR = 0X59 -ACCEL_OFFSET_Z_MSB_ADDR = 0X5A - -# Magnetometer Offset registers -MAG_OFFSET_X_LSB_ADDR = 0X5B -MAG_OFFSET_X_MSB_ADDR = 0X5C -MAG_OFFSET_Y_LSB_ADDR = 0X5D -MAG_OFFSET_Y_MSB_ADDR = 0X5E -MAG_OFFSET_Z_LSB_ADDR = 0X5F -MAG_OFFSET_Z_MSB_ADDR = 0X60 - -# Gyroscope Offset register s -GYRO_OFFSET_X_LSB_ADDR = 0X61 -GYRO_OFFSET_X_MSB_ADDR = 0X62 -GYRO_OFFSET_Y_LSB_ADDR = 0X63 -GYRO_OFFSET_Y_MSB_ADDR = 0X64 -GYRO_OFFSET_Z_LSB_ADDR = 0X65 -GYRO_OFFSET_Z_MSB_ADDR = 0X66 - -# Radius registers -ACCEL_RADIUS_LSB_ADDR = 0X67 -ACCEL_RADIUS_MSB_ADDR = 0X68 -MAG_RADIUS_LSB_ADDR = 0X69 -MAG_RADIUS_MSB_ADDR = 0X6A - -# Power modes -POWER_MODE_NORMAL = 0X00 -POWER_MODE_LOWPOWER = 0X01 -POWER_MODE_SUSPEND = 0X02 - -# Operation mode settings -OPERATION_MODE_CONFIG = 0X00 -OPERATION_MODE_ACCONLY = 0X01 -OPERATION_MODE_MAGONLY = 0X02 -OPERATION_MODE_GYRONLY = 0X03 -OPERATION_MODE_ACCMAG = 0X04 -OPERATION_MODE_ACCGYRO = 0X05 -OPERATION_MODE_MAGGYRO = 0X06 -OPERATION_MODE_AMG = 0X07 -OPERATION_MODE_IMUPLUS = 0X08 -OPERATION_MODE_COMPASS = 0X09 -OPERATION_MODE_M4G = 0X0A -OPERATION_MODE_NDOF_FMC_OFF = 0X0B -OPERATION_MODE_NDOF = 0X0C - - -logger = logging.getLogger(__name__) - - -class BNO055(object): - - def __init__(self, rst=None, address=BNO055_ADDRESS_A, i2c=None, gpio=None, - serial_port=None, serial_timeout_sec=5, **kwargs): - # If reset pin is provided save it and a reference to provided GPIO - # bus (or the default system GPIO bus if none is provided). - self._rst = rst - if self._rst is not None: - if gpio is None: - import Adafruit_GPIO as GPIO - gpio = GPIO.get_platform_gpio() - self._gpio = gpio - # Setup the reset pin as an output at a high level. - self._gpio.setup(self._rst, GPIO.OUT) - self._gpio.set_high(self._rst) - # Wait a 650 milliseconds in case setting the reset high reset the chip. - time.sleep(0.65) - self._serial = None - self._i2c_device = None - if serial_port is not None: - # Use serial communication if serial_port name is provided. - # Open the serial port at 115200 baud, 8N1. Add a 5 second timeout - # to prevent hanging if device is disconnected. - self._serial = serial.Serial(serial_port, 115200, timeout=serial_timeout_sec, - writeTimeout=serial_timeout_sec) - else: - # Use I2C if no serial port is provided. - # Assume we're using platform's default I2C bus if none is specified. - if i2c is None: - import Adafruit_GPIO.I2C as I2C - i2c = I2C - # Save a reference to the I2C device instance for later communication. - self._i2c_device = i2c.get_i2c_device(address, **kwargs) - - def _serial_send(self, command, ack=True, max_attempts=5): - # Send a serial command and automatically handle if it needs to be resent - # because of a bus error. If ack is True then an ackowledgement is - # expected and only up to the maximum specified attempts will be made - # to get a good acknowledgement (default is 5). If ack is False then - # no acknowledgement is expected (like when resetting the device). - attempts = 0 - while True: - # Flush any pending received data to get into a clean state. - self._serial.flushInput() - # Send the data. - self._serial.write(command) - logger.debug('Serial send: 0x{0}'.format(binascii.hexlify(command))) - # Stop if no acknowledgment is expected. - if not ack: - return - # Read acknowledgement response (2 bytes). - resp = bytearray(self._serial.read(2)) - logger.debug('Serial receive: 0x{0}'.format(binascii.hexlify(resp))) - if resp is None or len(resp) != 2: - raise RuntimeError('Timeout waiting for serial acknowledge, is the BNO055 connected?') - # Stop if there's no bus error (0xEE07 response) and return response bytes. - if not (resp[0] == 0xEE and resp[1] == 0x07): - return resp - # Else there was a bus error so resend, as recommended in UART app - # note at: - # http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST-BNO055-AN012-00.pdf - attempts += 1 - if attempts >= max_attempts: - raise RuntimeError('Exceeded maximum attempts to acknowledge serial command without bus error!') - - def _write_bytes(self, address, data, ack=True): - # Write a list of 8-bit values starting at the provided register address. - if self._i2c_device is not None: - # I2C write. - self._i2c_device.writeList(address, data) - else: - # Build and send serial register write command. - command = bytearray(4+len(data)) - command[0] = 0xAA # Start byte - command[1] = 0x00 # Write - command[2] = address & 0xFF - command[3] = len(data) & 0xFF - command[4:] = map(lambda x: x & 0xFF, data) - resp = self._serial_send(command, ack=ack) - # Verify register write succeeded if there was an acknowledgement. - if resp[0] != 0xEE and resp[1] != 0x01: - raise RuntimeError('Register write error: 0x{0}'.format(binascii.hexlify(resp))) - - def _write_byte(self, address, value, ack=True): - # Write an 8-bit value to the provided register address. If ack is True - # then expect an acknowledgement in serial mode, otherwise ignore any - # acknowledgement (necessary when resetting the device). - if self._i2c_device is not None: - # I2C write. - self._i2c_device.write8(address, value) - else: - # Build and send serial register write command. - command = bytearray(5) - command[0] = 0xAA # Start byte - command[1] = 0x00 # Write - command[2] = address & 0xFF - command[3] = 1 # Length (1 byte) - command[4] = value & 0xFF - resp = self._serial_send(command, ack=ack) - # Verify register write succeeded if there was an acknowledgement. - if ack and resp[0] != 0xEE and resp[1] != 0x01: - raise RuntimeError('Register write error: 0x{0}'.format(binascii.hexlify(resp))) - - def _read_bytes(self, address, length): - # Read a number of unsigned byte values starting from the provided address. - if self._i2c_device is not None: - # I2C read. - return bytearray(self._i2c_device.readList(address, length)) - else: - # Build and send serial register read command. - command = bytearray(4) - command[0] = 0xAA # Start byte - command[1] = 0x01 # Read - command[2] = address & 0xFF - command[3] = length & 0xFF - resp = self._serial_send(command) - # Verify register read succeeded. - if resp[0] != 0xBB: - raise RuntimeError('Register read error: 0x{0}'.format(binascii.hexlify(resp))) - # Read the returned bytes. - length = resp[1] - resp = bytearray(self._serial.read(length)) - logger.debug('Received: 0x{0}'.format(binascii.hexlify(resp))) - if resp is None or len(resp) != length: - raise RuntimeError('Timeout waiting to read data, is the BNO055 connected?') - return resp - - def _read_byte(self, address): - # Read an 8-bit unsigned value from the provided register address. - if self._i2c_device is not None: - # I2C read. - return self._i2c_device.readU8(address) - else: - return self._read_bytes(address, 1)[0] - - def _read_signed_byte(self, address): - # Read an 8-bit signed value from the provided register address. - data = self._read_byte(address) - if data > 127: - return data - 256 - else: - return data - - def _config_mode(self): - # Enter configuration mode. - self.set_mode(OPERATION_MODE_CONFIG) - - def _operation_mode(self): - # Enter operation mode to read sensor data. - self.set_mode(self._mode) - - def begin(self, mode=OPERATION_MODE_NDOF): - """Initialize the BNO055 sensor. Must be called once before any other - BNO055 library functions. Will return True if the BNO055 was - successfully initialized, and False otherwise. - """ - # Save the desired normal operation mode. - self._mode = mode - # First send a thow-away command and ignore any response or I2C errors - # just to make sure the BNO is in a good state and ready to accept - # commands (this seems to be necessary after a hard power down). - try: - self._write_byte(BNO055_PAGE_ID_ADDR, 0, ack=False) - except IOError: - # Swallow an IOError that might be raised by an I2C issue. Only do - # this for this very first command to help get the BNO and board's - # I2C into a clear state ready to accept the next commands. - pass - # Make sure we're in config mode and on page 0. - self._config_mode() - self._write_byte(BNO055_PAGE_ID_ADDR, 0) - # Check the chip ID - bno_id = self._read_byte(BNO055_CHIP_ID_ADDR) - logger.debug('Read chip ID: 0x{0:02X}'.format(bno_id)) - if bno_id != BNO055_ID: - return False - # Reset the device. - if self._rst is not None: - # Use the hardware reset pin if provided. - # Go low for a short period, then high to signal a reset. - self._gpio.set_low(self._rst) - time.sleep(0.01) # 10ms - self._gpio.set_high(self._rst) - else: - # Else use the reset command. Note that ack=False is sent because - # the chip doesn't seem to ack a reset in serial mode (by design?). - self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x20, ack=False) - # Wait 650ms after reset for chip to be ready (as suggested - # in datasheet). - time.sleep(0.65) - # Set to normal power mode. - self._write_byte(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL) - # Default to internal oscillator. - self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x0) - # Enter normal operation mode. - self._operation_mode() - return True - - def set_mode(self, mode): - """Set operation mode for BNO055 sensor. Mode should be a value from - table 3-3 and 3-5 of the datasheet: - http://www.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf - """ - self._write_byte(BNO055_OPR_MODE_ADDR, mode & 0xFF) - # Delay for 30 milliseconds (datsheet recommends 19ms, but a little more - # can't hurt and the kernel is going to spend some unknown amount of time - # too). - time.sleep(0.03) - - def get_revision(self): - """Return a tuple with revision information about the BNO055 chip. Will - return 5 values: - - Software revision - - Bootloader version - - Accelerometer ID - - Magnetometer ID - - Gyro ID - """ - # Read revision values. - accel = self._read_byte(BNO055_ACCEL_REV_ID_ADDR) - mag = self._read_byte(BNO055_MAG_REV_ID_ADDR) - gyro = self._read_byte(BNO055_GYRO_REV_ID_ADDR) - bl = self._read_byte(BNO055_BL_REV_ID_ADDR) - sw_lsb = self._read_byte(BNO055_SW_REV_ID_LSB_ADDR) - sw_msb = self._read_byte(BNO055_SW_REV_ID_MSB_ADDR) - sw = ((sw_msb << 8) | sw_lsb) & 0xFFFF - # Return the results as a tuple of all 5 values. - return (sw, bl, accel, mag, gyro) - - def set_external_crystal(self, external_crystal): - """Set if an external crystal is being used by passing True, otherwise - use the internal oscillator by passing False (the default behavior). - """ - # Switch to configuration mode. - self._config_mode() - # Set the clock bit appropriately in the SYS_TRIGGER register. - if external_crystal: - self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x80) - else: - self._write_byte(BNO055_SYS_TRIGGER_ADDR, 0x00) - # Go back to normal operation mode. - self._operation_mode() - - def get_system_status(self, run_self_test=True): - """Return a tuple with status information. Three values will be returned: - - System status register value with the following meaning: - 0 = Idle - 1 = System Error - 2 = Initializing Peripherals - 3 = System Initialization - 4 = Executing Self-Test - 5 = Sensor fusion algorithm running - 6 = System running without fusion algorithms - - Self test result register value with the following meaning: - Bit value: 1 = test passed, 0 = test failed - Bit 0 = Accelerometer self test - Bit 1 = Magnetometer self test - Bit 2 = Gyroscope self test - Bit 3 = MCU self test - Value of 0x0F = all good! - - System error register value with the following meaning: - 0 = No error - 1 = Peripheral initialization error - 2 = System initialization error - 3 = Self test result failed - 4 = Register map value out of range - 5 = Register map address out of range - 6 = Register map write error - 7 = BNO low power mode not available for selected operation mode - 8 = Accelerometer power mode not available - 9 = Fusion algorithm configuration error - 10 = Sensor configuration error - If run_self_test is passed in as False then no self test is performed and - None will be returned for the self test result. Note that running a - self test requires going into config mode which will stop the fusion - engine from running. - """ - self_test = None - if run_self_test: - # Switch to configuration mode if running self test. - self._config_mode() - # Perform a self test. - sys_trigger = self._read_byte(BNO055_SYS_TRIGGER_ADDR) - self._write_byte(BNO055_SYS_TRIGGER_ADDR, sys_trigger | 0x1) - # Wait for self test to finish. - time.sleep(1.0) - # Read test result. - self_test = self._read_byte(BNO055_SELFTEST_RESULT_ADDR) - # Go back to operation mode. - self._operation_mode() - # Now read status and error registers. - status = self._read_byte(BNO055_SYS_STAT_ADDR) - error = self._read_byte(BNO055_SYS_ERR_ADDR) - # Return the results as a tuple of all 3 values. - return (status, self_test, error) - - def get_calibration_status(self): - """Read the calibration status of the sensors and return a 4 tuple with - calibration status as follows: - - System, 3=fully calibrated, 0=not calibrated - - Gyroscope, 3=fully calibrated, 0=not calibrated - - Accelerometer, 3=fully calibrated, 0=not calibrated - - Magnetometer, 3=fully calibrated, 0=not calibrated - """ - # Return the calibration status register value. - cal_status = self._read_byte(BNO055_CALIB_STAT_ADDR) - sys = (cal_status >> 6) & 0x03 - gyro = (cal_status >> 4) & 0x03 - accel = (cal_status >> 2) & 0x03 - mag = cal_status & 0x03 - # Return the results as a tuple of all 3 values. - return (sys, gyro, accel, mag) - - def get_calibration(self): - """Return the sensor's calibration data and return it as an array of - 22 bytes. Can be saved and then reloaded with the set_calibration function - to quickly calibrate from a previously calculated set of calibration data. - """ - # Switch to configuration mode, as mentioned in section 3.10.4 of datasheet. - self._config_mode() - # Read the 22 bytes of calibration data and convert it to a list (from - # a bytearray) so it's more easily serialized should the caller want to - # store it. - cal_data = list(self._read_bytes(ACCEL_OFFSET_X_LSB_ADDR, 22)) - # Go back to normal operation mode. - self._operation_mode() - return cal_data - - def set_calibration(self, data): - """Set the sensor's calibration data using a list of 22 bytes that - represent the sensor offsets and calibration data. This data should be - a value that was previously retrieved with get_calibration (and then - perhaps persisted to disk or other location until needed again). - """ - # Check that 22 bytes were passed in with calibration data. - if data is None or len(data) != 22: - raise ValueError('Expected a list of 22 bytes for calibration data.') - # Switch to configuration mode, as mentioned in section 3.10.4 of datasheet. - self._config_mode() - # Set the 22 bytes of calibration data. - self._write_bytes(ACCEL_OFFSET_X_LSB_ADDR, data) - # Go back to normal operation mode. - self._operation_mode() - - def get_axis_remap(self): - """Return a tuple with the axis remap register values. This will return - 6 values with the following meaning: - - X axis remap (a value of AXIS_REMAP_X, AXIS_REMAP_Y, or AXIS_REMAP_Z. - which indicates that the physical X axis of the chip - is remapped to a different axis) - - Y axis remap (see above) - - Z axis remap (see above) - - X axis sign (a value of AXIS_REMAP_POSITIVE or AXIS_REMAP_NEGATIVE - which indicates if the X axis values should be positive/ - normal or negative/inverted. The default is positive.) - - Y axis sign (see above) - - Z axis sign (see above) - Note that by default the axis orientation of the BNO chip looks like - the following (taken from section 3.4, page 24 of the datasheet). Notice - the dot in the corner that corresponds to the dot on the BNO chip: - | Z axis - | - | / X axis - ____|__/____ - Y axis / * | / /| - _________ /______|/ // - /___________ // - |____________|/ - """ - # Get the axis remap register value. - map_config = self._read_byte(BNO055_AXIS_MAP_CONFIG_ADDR) - z = (map_config >> 4) & 0x03 - y = (map_config >> 2) & 0x03 - x = map_config & 0x03 - # Get the axis remap sign register value. - sign_config = self._read_byte(BNO055_AXIS_MAP_SIGN_ADDR) - x_sign = (sign_config >> 2) & 0x01 - y_sign = (sign_config >> 1) & 0x01 - z_sign = sign_config & 0x01 - # Return the results as a tuple of all 3 values. - return (x, y, z, x_sign, y_sign, z_sign) - - def set_axis_remap(self, x, y, z, - x_sign=AXIS_REMAP_POSITIVE, y_sign=AXIS_REMAP_POSITIVE, - z_sign=AXIS_REMAP_POSITIVE): - """Set axis remap for each axis. The x, y, z parameter values should - be set to one of AXIS_REMAP_X, AXIS_REMAP_Y, or AXIS_REMAP_Z and will - change the BNO's axis to represent another axis. Note that two axises - cannot be mapped to the same axis, so the x, y, z params should be a - unique combination of AXIS_REMAP_X, AXIS_REMAP_Y, AXIS_REMAP_Z values. - The x_sign, y_sign, z_sign values represent if the axis should be positive - or negative (inverted). - See the get_axis_remap documentation for information on the orientation - of the axises on the chip, and consult section 3.4 of the datasheet. - """ - # Switch to configuration mode. - self._config_mode() - # Set the axis remap register value. - map_config = 0x00 - map_config |= (z & 0x03) << 4 - map_config |= (y & 0x03) << 2 - map_config |= x & 0x03 - self._write_byte(BNO055_AXIS_MAP_CONFIG_ADDR, map_config) - # Set the axis remap sign register value. - sign_config = 0x00 - sign_config |= (x_sign & 0x01) << 2 - sign_config |= (y_sign & 0x01) << 1 - sign_config |= z_sign & 0x01 - self._write_byte(BNO055_AXIS_MAP_SIGN_ADDR, sign_config) - # Go back to normal operation mode. - self._operation_mode() - - def _read_vector(self, address, count=3): - # Read count number of 16-bit signed values starting from the provided - # address. Returns a tuple of the values that were read. - data = self._read_bytes(address, count*2) - result = [0]*count - for i in range(count): - result[i] = ((data[i*2+1] << 8) | data[i*2]) & 0xFFFF - if result[i] > 32767: - result[i] -= 65536 - return result - - def read_euler(self): - """Return the current absolute orientation as a tuple of heading, roll, - and pitch euler angles in degrees. - """ - heading, roll, pitch = self._read_vector(BNO055_EULER_H_LSB_ADDR) - return (heading/16.0, roll/16.0, pitch/16.0) - - def read_magnetometer(self): - """Return the current magnetometer reading as a tuple of X, Y, Z values - in micro-Teslas. - """ - x, y, z = self._read_vector(BNO055_MAG_DATA_X_LSB_ADDR) - return (x/16.0, y/16.0, z/16.0) - - def read_gyroscope(self): - """Return the current gyroscope (angular velocity) reading as a tuple of - X, Y, Z values in degrees per second. - """ - x, y, z = self._read_vector(BNO055_GYRO_DATA_X_LSB_ADDR) - return (x/900.0, y/900.0, z/900.0) - - def read_accelerometer(self): - """Return the current accelerometer reading as a tuple of X, Y, Z values - in meters/second^2. - """ - x, y, z = self._read_vector(BNO055_ACCEL_DATA_X_LSB_ADDR) - return (x/100.0, y/100.0, z/100.0) - - def read_linear_acceleration(self): - """Return the current linear acceleration (acceleration from movement, - not from gravity) reading as a tuple of X, Y, Z values in meters/second^2. - """ - x, y, z = self._read_vector(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR) - return (x/100.0, y/100.0, z/100.0) - - def read_gravity(self): - """Return the current gravity acceleration reading as a tuple of X, Y, Z - values in meters/second^2. - """ - x, y, z = self._read_vector(BNO055_GRAVITY_DATA_X_LSB_ADDR) - return (x/100.0, y/100.0, z/100.0) - - def read_quaternion(self): - """Return the current orientation as a tuple of X, Y, Z, W quaternion - values. - """ - w, x, y, z = self._read_vector(BNO055_QUATERNION_DATA_W_LSB_ADDR, 4) - # Scale values, see 3.6.5.5 in the datasheet. - scale = (1.0 / (1<<14)) - return (x*scale, y*scale, z*scale, w*scale) - - def read_temp(self): - """Return the current temperature in Celsius.""" - return self._read_signed_byte(BNO055_TEMP_ADDR) \ No newline at end of file diff --git a/ros/src/i2c_proc/scripts/depth_proc.py b/ros/src/i2c_proc/scripts/depth_proc.py deleted file mode 100755 index a736eda..0000000 --- a/ros/src/i2c_proc/scripts/depth_proc.py +++ /dev/null @@ -1,31 +0,0 @@ -#! /usr/bin/python -import rospy -# import ms5837 -import ms5837 -from std_msgs.msg import Float32 - -if __name__ == "__main__": - rospy.init_node('depth_proc') - - depth_sensor = None - try: - depth_sensor = ms5837.MS5837(1) # Initialize sensor on i2c bus 1 - depth_sensor.init() # Initializes with density of freshwater - except: - - pass - - pub = rospy.Publisher('depth', - Float32, queue_size=10) - - rate = rospy.Rate(10) # 10hz - # TODO: I2C related activities - while not rospy.is_shutdown(): - try: - depth_sensor.read() #allows the sensor to read new data - depth = depth_sensor.depth() - - except: - depth = 0 - pub.publish(Float32(depth)) - rate.sleep() diff --git a/ros/src/i2c_proc/scripts/imu_proc.py b/ros/src/i2c_proc/scripts/imu_proc.py deleted file mode 100755 index 2f3755b..0000000 --- a/ros/src/i2c_proc/scripts/imu_proc.py +++ /dev/null @@ -1,104 +0,0 @@ -#! /usr/bin/python -import rospy -import smbus -import math -from BNO055 import BNO055 -from shared_msgs.msg import imu_msg -from std_msgs.msg import Bool -IMU_PITCH_OFFSET = 0.0 -IMU_ROLL_OFFSET = 0.0 -IMU_YAW_OFFSET = 0.0 -class dummyIMU(): - def __init__(self): - pass - @property - def data(self): - return_data = { - 'euler': { - # Resolution found from a forumn post - 'yaw': 0, # Rotation about z axis (vertical) +/- 0.01 degree - 'roll': 0, # Rotation about y axix (perpindicular to the pins IMU) +/- 0.01 degree - 'pitch': 0, # Rotation about x axis (parallel to the pins of IMU) +/- 0.01 degree - - }, - 'gyro': { - 'x': 0, # 3e-2 degree/sec - 'y': 0, # 3e-2 degree/sec - 'z': 0, # 3e-2 degree/sec - }, - 'acceleration': { - 'x': 0, # +/- 5e-4 g - 'y': 0, # +/- 5e-4 g - 'z': 0, # +/- 5e-4 g - }, - 'linear_acceleration': { - 'x': 0, # +/- 0.25 m/s^2 - 'y': 0, # +/- 0.25 m/s^2 - 'z': 0, # +/- 0.25 m/s^2 - }, - 'temp': 0, # Good enough - } -#try: - -#except: -# print "no sensor found" -# imu = dummyIMU(); - - - -def reset_imu_offsets(): - global imu - global IMU_PITCH_OFFSET - global IMU_ROLL_OFFSET - global IMU_YAW_OFFSET - print "message recieved" - IMU_PITCH_OFFSET = imu.pitch(); - IMU_ROLL_OFFSET = imu.roll(); - IMU_YAW_OFFSET = imu.yaw(); - print ("imu_pitch offset" , IMU_PITCH_OFFSET) - -#bind all angles to -180 to 180 -def clamp_angle_neg180_to_180(angle): - angle_0_to_360 = clamp_angle_0_to_360(angle) - if angle_0_to_360 > 180: - return angle_0_to_360 - 180 * -1.0 - return angle_0_to_360 -#bind all angles to -180 to 180 -def clamp_angle_0_to_360(angle): - return (angle + 1* 360) - math.floor((angle + 2 * 360)/360)*360 - -if __name__ == "__main__": - global imu - rospy.init_node('imu_proc') - imu = BNO055() - # Publish to the CAN hardware transmitter - pub = rospy.Publisher('imu', imu_msg, - queue_size=1) - - #sub = rospy.Subscriber('reset_imu', Bool, - # _reset_imu_offsets) - - rate = rospy.Rate(50) # 10hz - firstTime = 1000 - while not rospy.is_shutdown(): - if imu.update(): - out_message = imu_msg() - imu_data = imu.data - #if firstTime>0: - # firstTime -= 1 - # IMU_ROLL_OFFSET = imu.roll() - # IMU_YAW_OFFSET = imu.yaw() - # IMU_PITCH_OFFSET = imu.pitch() - #convert everything to a 0 to 360 to apply a 1d rotation then convert back to -180 to 180 - ROV_Pitch = clamp_angle_0_to_360(imu.roll()) - IMU_ROLL_OFFSET; - ROV_Roll = clamp_angle_0_to_360(imu.yaw()) - IMU_YAW_OFFSET; - ROV_Yaw = clamp_angle_0_to_360(imu.pitch()) - IMU_PITCH_OFFSET; - # out_message.gyro = [ROV_Pitch, ROV_Roll, ROV_Yaw] - out_message.gyro = [imu.roll(),imu.yaw(),imu.pitch()] - ROV_X_Accel = imu.acceleration_z(); - ROV_Y_Accel = imu.acceleration_x(); - ROV_Z_Accel = imu.acceleration_y(); - out_message.accel = [ROV_X_Accel, ROV_Y_Accel, ROV_Z_Accel] - pub.publish(out_message) - rate.sleep() - diff --git a/ros/src/i2c_proc/scripts/ms5837.py b/ros/src/i2c_proc/scripts/ms5837.py deleted file mode 100755 index e955db5..0000000 --- a/ros/src/i2c_proc/scripts/ms5837.py +++ /dev/null @@ -1,235 +0,0 @@ -try: - import smbus -except: - print 'Try sudo apt-get install python-smbus' - -from time import sleep - -# Models -MODEL_02BA = 0 -MODEL_30BA = 1 - -# Oversampling options -OSR_256 = 0 -OSR_512 = 1 -OSR_1024 = 2 -OSR_2048 = 3 -OSR_4096 = 4 -OSR_8192 = 5 - -# kg/m^3 convenience -DENSITY_FRESHWATER = 997 -DENSITY_SALTWATER = 1029 - -# Conversion factors (from native unit, mbar) -UNITS_Pa = 100.0 -UNITS_hPa = 1.0 -UNITS_kPa = 0.1 -UNITS_mbar = 1.0 -UNITS_bar = 0.001 -UNITS_atm = 0.000986923 -UNITS_Torr = 0.750062 -UNITS_psi = 0.014503773773022 - -# Valid units -UNITS_Centigrade = 1 -UNITS_Farenheit = 2 -UNITS_Kelvin = 3 - - -class MS5837(object): - - # Registers - _MS5837_ADDR = 0x76 - _MS5837_RESET = 0x1E - _MS5837_ADC_READ = 0x00 - _MS5837_PROM_READ = 0xA0 - _MS5837_CONVERT_D1_256 = 0x40 - _MS5837_CONVERT_D2_256 = 0x50 - - def __init__(self, model=MODEL_30BA, bus=1): - self._model = model - - try: - self._bus = smbus.SMBus(bus) - except: - print("Bus %d is not available.") % bus - print("Available busses are listed as /dev/i2c*") - self._bus = None - - self._fluidDensity = DENSITY_FRESHWATER - self._pressure = 0 - self._temperature = 0 - self._D1 = 0 - self._D2 = 0 - - def init(self): - if self._bus is None: - "No bus!" - return False - - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_RESET) - - # Wait for reset to complete - sleep(0.01) - - self._C = [] - - # Read calibration values and CRC - for i in range(7): - c = self._bus.read_word_data(self._MS5837_ADDR, self._MS5837_PROM_READ + 2*i) - c = ((c & 0xFF) << 8) | (c >> 8) # SMBus is little-endian for word transfers, we need to swap MSB and LSB - self._C.append(c) - - crc = (self._C[0] & 0xF000) >> 12 - if crc != self._crc4(self._C): - print "PROM read error, CRC failed!" - return False - - return True - - def read(self, oversampling=OSR_8192): - if self._bus is None: - print "No bus!" - return False - - if oversampling < OSR_256 or oversampling > OSR_8192: - print "Invalid oversampling option!" - return False - - # Request D1 conversion (temperature) - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D1_256 + 2*oversampling) - - # Maximum conversion time increases linearly with oversampling - # max time (seconds) ~= 2.2e-6(x) where x = OSR = (2^8, 2^9, ..., 2^13) - # We use 2.5e-6 for some overhead - sleep(2.5e-6 * 2**(8+oversampling)) - - d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) - self._D1 = d[0] << 16 | d[1] << 8 | d[2] - - # Request D2 conversion (pressure) - self._bus.write_byte(self._MS5837_ADDR, self._MS5837_CONVERT_D2_256 + 2*oversampling) - - # As above - sleep(2.5e-6 * 2**(8+oversampling)) - - d = self._bus.read_i2c_block_data(self._MS5837_ADDR, self._MS5837_ADC_READ, 3) - self._D2 = d[0] << 16 | d[1] << 8 | d[2] - - # Calculate compensated pressure and temperature - # using raw ADC values and internal calibration - self._calculate() - - return True - - def setFluidDensity(self, denisty): - self._fluidDensity = denisty - - # Pressure in requested units - # mbar * conversion - def pressure(self, conversion=UNITS_mbar): - return self._pressure * conversion - - # Temperature in requested units - # default degrees C - def temperature(self, conversion=UNITS_Centigrade): - degC = self._temperature / 100.0 - if conversion == UNITS_Farenheit: - return (9.0/5.0)*degC + 32 - elif conversion == UNITS_Kelvin: - return degC + 273 - return degC - - # Depth relative to MSL pressure in given fluid density - def depth(self): - return (self.pressure(UNITS_Pa)-101300)/(self._fluidDensity*9.80665) - - # Altitude relative to MSL pressure - def altitude(self): - return (1-pow((self.pressure()/1013.25),.190284))*145366.45*.3048 - - # Cribbed from datasheet - def _calculate(self): - OFFi = 0 - SENSi = 0 - Ti = 0 - - dT = self._D2-self._C[5]*256 - if self._model == MODEL_02BA: - SENS = self._C[1]*65536+(self._C[3]*dT)/128 - OFF = self._C[2]*131072+(self._C[4]*dT)/64 - self._pressure = (self._D1*SENS/(2097152)-OFF)/(32768) - else: - SENS = self._C[1]*32768+(self._C[3]*dT)/256 - OFF = self._C[2]*65536+(self._C[4]*dT)/128 - self._pressure = (self._D1*SENS/(2097152)-OFF)/(8192) - - self._temperature = 2000+dT*self._C[6]/8388608 - - # Second order compensation - if self._model == MODEL_02BA: - if (self._temperature/100) < 20: # Low temp - Ti = (11*dT*dT)/(34359738368) - OFFi = (31*(self._temperature-2000)*(self._temperature-2000))/8 - SENSi = (63*(self._temperature-2000)*(self._temperature-2000))/32 - - else: - if (self._temperature/100) < 20: # Low temp - Ti = (3*dT*dT)/(8589934592) - OFFi = (3*(self._temperature-2000)*(self._temperature-2000))/2 - SENSi = (5*(self._temperature-2000)*(self._temperature-2000))/8 - if (self._temperature/100) < -15: # Very low temp - OFFi = OFFi+7*(self._temperature+1500l)*(self._temperature+1500) - SENSi = SENSi+4*(self._temperature+1500l)*(self._temperature+1500) - elif (self._temperature/100) >= 20: # High temp - Ti = 2*(dT*dT)/(137438953472) - OFFi = (1*(self._temperature-2000)*(self._temperature-2000))/16 - SENSi = 0 - - OFF2 = OFF-OFFi - SENS2 = SENS-SENSi - - if self._model == MODEL_02BA: - self._temperature = (self._temperature-Ti) - self._pressure = (((self._D1*SENS2)/2097152-OFF2)/32768)/100.0 - else: - self._temperature = (self._temperature-Ti) - self._pressure = (((self._D1*SENS2)/2097152-OFF2)/8192)/10.0 - - # Cribbed from datasheet - def _crc4(self, n_prom): - n_rem = 0 - - n_prom[0] = ((n_prom[0]) & 0x0FFF) - n_prom.append(0) - - for i in range(16): - if i%2 == 1: - n_rem ^= ((n_prom[i>>1]) & 0x00FF) - else: - n_rem ^= (n_prom[i>>1] >> 8) - - for n_bit in range(8,0,-1): - if n_rem & 0x8000: - n_rem = (n_rem << 1) ^ 0x3000 - else: - n_rem = (n_rem << 1) - - n_rem = ((n_rem >> 12) & 0x000F) - - self.n_prom = n_prom - self.n_rem = n_rem - - return n_rem ^ 0x00 - -class MS5837_30BA(MS5837): - def __init__(self, bus=1): - MS5837.__init__(self, MODEL_30BA, bus) - -class MS5837_02BA(MS5837): - def __init__(self, bus=1): - MS5837.__init__(self, MODEL_02BA, bus) - - - \ No newline at end of file diff --git a/ros/src/i2c_proc/scripts/ph_proc.py b/ros/src/i2c_proc/scripts/ph_proc.py deleted file mode 100755 index 521ae74..0000000 --- a/ros/src/i2c_proc/scripts/ph_proc.py +++ /dev/null @@ -1,25 +0,0 @@ -#! /usr/bin/python -import rospy -from shared_msgs.msg import i2c_msg -from std_msgs.msg import Float32 - - -def message_received(msg): - # This runs on a seperate thread from the pub - pass - - -if __name__ == "__main__": - rospy.init_node('ph_proc') - - sub = rospy.Subscriber('i2c_rx', i2c_msg, - message_received) - pub = rospy.Publisher('ph', Float32, - queue_size=10); - - rate = rospy.Rate(10) # 10hz - - # TODO: I2C related activities - while not rospy.is_shutdown(): - pub.publish(Float32()) - rate.sleep() diff --git a/ros/src/i2c_proc/scripts/temp_proc.py b/ros/src/i2c_proc/scripts/temp_proc.py deleted file mode 100755 index afabbae..0000000 --- a/ros/src/i2c_proc/scripts/temp_proc.py +++ /dev/null @@ -1,27 +0,0 @@ -#! /usr/bin/python -import rospy -from shared_msgs.msg import i2c_msg -from sensor_msgs.msg import Temperature - - -def message_received(msg): - # This runs on a seperate thread from the pub - pass - - -if __name__ == "__main__": - rospy.init_node('temp_proc') - - # Publish to the CAN hardware transmitter - pub = rospy.Publisher('temp', Temperature, - queue_size=100) - - sub = rospy.Subscriber('i2c_rx', i2c_msg, - message_received) - - rate = rospy.Rate(10) # 10hz - # TODO: I2C related activities - while not rospy.is_shutdown(): - sample_message = Temperature() - pub.publish(sample_message) - rate.sleep() diff --git a/surface/ros/src/ramping/CMakeLists.txt b/ros/src/pi_temp/CMakeLists.txt similarity index 94% rename from surface/ros/src/ramping/CMakeLists.txt rename to ros/src/pi_temp/CMakeLists.txt index 817c2c4..0f0fb78 100644 --- a/surface/ros/src/ramping/CMakeLists.txt +++ b/ros/src/pi_temp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ramping) +project(pi_temp) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -10,7 +10,6 @@ project(ramping) find_package(catkin REQUIRED COMPONENTS roscpp rospy - shared_msgs std_msgs ) @@ -71,7 +70,7 @@ find_package(catkin REQUIRED COMPONENTS ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES -# shared_msgs# std_msgs +# std_msgs # ) ################################################ @@ -105,8 +104,8 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES ramping -# CATKIN_DEPENDS roscpp rospy shared_msgs std_msgs +# LIBRARIES pi_temp +# CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) @@ -123,7 +122,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/ramping.cpp +# src/${PROJECT_NAME}/pi_temp.cpp # ) ## Add cmake target dependencies of the library @@ -134,7 +133,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/ramping_node.cpp) +# add_executable(${PROJECT_NAME}_node src/pi_temp_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -160,10 +159,10 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS + src/pi_temp_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html @@ -198,7 +197,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_ramping.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pi_temp.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/surface/ros/src/ramping/package.xml b/ros/src/pi_temp/package.xml similarity index 89% rename from surface/ros/src/ramping/package.xml rename to ros/src/pi_temp/package.xml index 86e2ee0..8209e39 100644 --- a/surface/ros/src/ramping/package.xml +++ b/ros/src/pi_temp/package.xml @@ -1,13 +1,13 @@ - ramping + pi_temp 0.0.0 - Ramping + The pi_temp package - ivan + zach @@ -19,7 +19,7 @@ - + @@ -51,15 +51,12 @@ catkin roscpp rospy - shared_msgs std_msgs roscpp rospy - shared_msgs std_msgs roscpp rospy - shared_msgs std_msgs diff --git a/ros/src/pi_temp/src/pi_temp_node.py b/ros/src/pi_temp/src/pi_temp_node.py new file mode 100755 index 0000000..e993b2b --- /dev/null +++ b/ros/src/pi_temp/src/pi_temp_node.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python3 + +# license removed for brevity +import rospy +from gpiozero import CPUTemperature +from std_msgs.msg import String +from std_msgs.msg import Float64 + +def talker(): + #collect's pi's cpu temperature data in degrees C. + cpu = CPUTemperature() + pub = rospy.Publisher("Pi_TEMP", Float64, queue_size=10) + rospy.init_node('Pi_CPU_TEMP', anonymous=True) + rate = rospy.Rate(1) # 10hz + + while not rospy.is_shutdown(): + rospy.loginfo(cpu.temperature) + pub.publish(cpu.temperature) + rate.sleep() + +if __name__ == '__main__': + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/ros/src/pid b/ros/src/pid deleted file mode 160000 index 8268b72..0000000 --- a/ros/src/pid +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8268b7250e40c33a08d7faa7706885eb317eadbd diff --git a/ros/src/pid/CHANGELOG.rst b/ros/src/pid/CHANGELOG.rst new file mode 100644 index 0000000..24f99ce --- /dev/null +++ b/ros/src/pid/CHANGELOG.rst @@ -0,0 +1,89 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pid +^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.28 (2020-07-03) +------------------- +* Fix compatibility issues when building with ROS Noetic +* Bug fix for angle wrapping always resetting filtered and integral error +* Adding a parameter setpoint_timeout which specifies whether to keep publishing control_effort messages after setpoint messages stop coming in +* Add link to low-pass filter reference +* Cleaning up package.xml just a bit more. +* Removing a bunch of superfluous dependencies. +* Cleaning CMakeLists. +* Upgrade to package format=2 +* Contributors: Andrew J Zelenak, AndyZe, Stewart Jamieson, Vikrant Shah + +0.0.15 (2016-02-22) +------------------- +* Fixing header "include" error, possible dynamic_reconfigure errors. +* Contributors: Andy Zelenak + +0.0.10 (2016-1-25) +------------------ +* Add diagnostics running at 4Hz. Publish interesting data like setpoint, plant state, +error, control effort in diags for viewing +* Add ROS private parameters to set Pid params and simulator param +* Add Auto/Manual mode: Listen to the /pid_enable topic for a std_msgs/Bool +that will disable or re-enable output from the PID controller +* Support reverse_acting parameter, for plants where the control input acts +in the opposite direction to the plant-state. E.g. Differential-drive robots +often have one direct and one reverse-acting PID +* Support faster-than-wallclock simulation via a /clock publisher and the +/use_sim_time parameter +* Add launch files for first & 2nd order behaviors. Get them to launch plots, +diag monitor, reconfigure gui +* Split setpoint generator out into a separate node +* Rename simulator to plant_sim.cpp & give it 1st & 2nd order behaviors, configured +with a parameter. +* Delete first_order_plant_sim.cpp, which is subsumed into plant_sim.cpp. Remove +plant header, which was almost content-free. +* Remove msg directory & switch to using Float64 messages for setpoint, +process state, & control effort. Now it's generic - no special messages needed. +* Explicitly call out std namespace to avoid accidental name conflicts +* Remove parameter length checks to allow parameters to be set in part from +launch file (the usual way), and in part from cmd line args (an infrequently-used +way) +* Rename pid_header.h to controller.h because it's used by controller.cpp - more standard +C++ naming style +* Add copyrights +* Contributors: AndyZe, Paul Bouchier + +0.0.9 (2015-12-27) +------------------ +* Merged in bouchier/pid (pull request #1) + Add dynamic reconfigure for Kx, change plant setpoint +* add dynamic reconfigure for Kx, change plant setpoint +* Contributors: AndyZe, Paul Bouchier + +0.0.7 (2015-07-26) +------------------ +* Added a launch file, which required the arguments to be processed differently. +* Contributors: Andy Zelenak + +0.0.6 (2015-06-09) +------------------ +* Changing the way parameters are passed to check_user_input(). +* Contributors: Andy Zelenak + +0.0.5 (2015-06-09) +------------------ +* Adding an anti-windup option. +* Contributors: Andy Zelenak + +0.0.3 (2015-03-14) +------------------ + +0.0.2 (2015-03-13) +------------------ + +0.0.1 (2015-03-08) +------------------ +* Fixing various minor bugs related to user input. +* Pre-release commit. +* It WORKS! +* It's talking with the plant sim now. Just the PID programming remains. +* Making progress with the command line input. +* Rough outline of the program. Need to take inputs on the command line next. +* Initial commit +* Contributors: Andy Zelenak diff --git a/ros/src/pid/CMakeLists.txt b/ros/src/pid/CMakeLists.txt new file mode 100644 index 0000000..2fbeae5 --- /dev/null +++ b/ros/src/pid/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pid) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + dynamic_reconfigure +) +find_package(Boost REQUIRED COMPONENTS system) + +generate_dynamic_reconfigure_options( + cfg/Pid.cfg +) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp std_msgs message_runtime dynamic_reconfigure +) + +include_directories(include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake +) + +add_executable(controller src/controller.cpp src/pid.cpp) +add_executable(plant_sim src/plant_sim.cpp) +add_executable(setpoint_node src/setpoint_node.cpp) +add_executable(sim_time src/sim_time.cpp) +add_executable(autotune src/autotune.cpp) + +add_dependencies(controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(plant_sim ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(setpoint_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(sim_time ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(autotune ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(controller ${catkin_LIBRARIES}) +target_link_libraries(plant_sim ${catkin_LIBRARIES}) +target_link_libraries(setpoint_node ${catkin_LIBRARIES}) +target_link_libraries(sim_time ${catkin_LIBRARIES}) +target_link_libraries(autotune ${catkin_LIBRARIES}) + +install(TARGETS controller plant_sim setpoint_node sim_time autotune + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/ros/src/pid/cfg/Pid.cfg b/ros/src/pid/cfg/Pid.cfg new file mode 100755 index 0000000..9466710 --- /dev/null +++ b/ros/src/pid/cfg/Pid.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python +PACKAGE = "pid" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() +K_scale_enum = gen.enum([ gen.const("scale_tenth", double_t, 0.1, "Scale by 0.1"), + gen.const("scale_unity", double_t, 1.0, "No scaling"), + gen.const("scale_ten", double_t, 10.0, "Scale by 10"), + gen.const("scale_hundred", double_t, 100.0, "Scale by 100")], + "Scale factor for K setting") + +gen.add("Kp_scale", double_t, 0, "Kp scale", 10, 0.1, 100, edit_method = K_scale_enum) +gen.add("Kp", double_t, 0, "Kp", 0.1, -1, 1) +gen.add("Ki_scale", double_t, 0, "Ki scale", 10, 0.1, 100, edit_method = K_scale_enum) +gen.add("Ki", double_t, 0, "Ki", 0.1, -1, 1) +gen.add("Kd_scale", double_t, 0, "Kd scale", 10, 0.1, 100, edit_method = K_scale_enum) +gen.add("Kd", double_t, 0, "Kd", 0.1, -1, 1) + +exit(gen.generate(PACKAGE, "pid", "Pid")) diff --git a/ros/src/pid/contributors.txt b/ros/src/pid/contributors.txt new file mode 100644 index 0000000..9569179 --- /dev/null +++ b/ros/src/pid/contributors.txt @@ -0,0 +1,2 @@ +Andy Zelenak +Paul Bouchier diff --git a/ros/src/pid/include/pid/PidConfig.h b/ros/src/pid/include/pid/PidConfig.h new file mode 100644 index 0000000..d69acab --- /dev/null +++ b/ros/src/pid/include/pid/PidConfig.h @@ -0,0 +1,736 @@ +//#line 2 +//"/opt/ros/indigo/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" +// ********************************************************* +// +// File autogenerated for the pid package +// by the dynamic_reconfigure package. +// Please do not edit. +// +// ********************************************************/ + +#include +#include +#include + +#ifndef __pid__PIDCONFIG_H__ +#define __pid__PIDCONFIG_H__ + +namespace pid +{ +class PidConfigStatics; + +class PidConfig +{ +public: + class AbstractParamDescription : public dynamic_reconfigure::ParamDescription + { + public: + AbstractParamDescription(std::string n, std::string t, uint32_t l, std::string d, std::string e) + { + name = n; + type = t; + level = l; + description = d; + edit_method = e; + } + + virtual void clamp(PidConfig& config, const PidConfig& max, const PidConfig& min) const = 0; + virtual void calcLevel(uint32_t& level, const PidConfig& config1, const PidConfig& config2) const = 0; + virtual void fromServer(const ros::NodeHandle& nh, PidConfig& config) const = 0; + virtual void toServer(const ros::NodeHandle& nh, const PidConfig& config) const = 0; + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, PidConfig& config) const = 0; + virtual void toMessage(dynamic_reconfigure::Config& msg, const PidConfig& config) const = 0; + virtual void getValue(const PidConfig& config, boost::any& val) const = 0; + }; + + typedef boost::shared_ptr AbstractParamDescriptionPtr; + typedef boost::shared_ptr AbstractParamDescriptionConstPtr; + + template + class ParamDescription : public AbstractParamDescription + { + public: + ParamDescription(std::string name, std::string type, uint32_t level, std::string description, + std::string edit_method, T PidConfig::*f) + : AbstractParamDescription(name, type, level, description, edit_method), field(f) + { + } + + T(PidConfig::*field); + + virtual void clamp(PidConfig& config, const PidConfig& max, const PidConfig& min) const + { + if (config.*field > max.*field) + config.*field = max.*field; + + if (config.*field < min.*field) + config.*field = min.*field; + } + + virtual void calcLevel(uint32_t& comb_level, const PidConfig& config1, const PidConfig& config2) const + { + if (config1.*field != config2.*field) + comb_level |= level; + } + + virtual void fromServer(const ros::NodeHandle& nh, PidConfig& config) const + { + nh.getParam(name, config.*field); + } + + virtual void toServer(const ros::NodeHandle& nh, const PidConfig& config) const + { + nh.setParam(name, config.*field); + } + + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, PidConfig& config) const + { + return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field); + } + + virtual void toMessage(dynamic_reconfigure::Config& msg, const PidConfig& config) const + { + dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field); + } + + virtual void getValue(const PidConfig& config, boost::any& val) const + { + val = config.*field; + } + }; + + class AbstractGroupDescription : public dynamic_reconfigure::Group + { + public: + AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s) + { + name = n; + type = t; + parent = p; + state = s; + id = i; + } + + std::vector abstract_parameters; + bool state; + + virtual void toMessage(dynamic_reconfigure::Config& msg, const boost::any& config) const = 0; + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, boost::any& config) const = 0; + virtual void updateParams(boost::any& cfg, PidConfig& top) const = 0; + virtual void setInitialState(boost::any& cfg) const = 0; + + void convertParams() + { + for (std::vector::const_iterator i = abstract_parameters.begin(); + i != abstract_parameters.end(); ++i) + { + parameters.push_back(dynamic_reconfigure::ParamDescription(**i)); + } + } + }; + + typedef boost::shared_ptr AbstractGroupDescriptionPtr; + typedef boost::shared_ptr AbstractGroupDescriptionConstPtr; + + template + class GroupDescription : public AbstractGroupDescription + { + public: + GroupDescription(std::string name, std::string type, int parent, int id, bool s, T PT::*f) + : AbstractGroupDescription(name, type, parent, id, s), field(f) + { + } + + GroupDescription(const GroupDescription& g) + : AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups) + { + parameters = g.parameters; + abstract_parameters = g.abstract_parameters; + } + + virtual bool fromMessage(const dynamic_reconfigure::Config& msg, boost::any& cfg) const + { + PT* config = boost::any_cast(cfg); + if (!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field)) + return false; + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = &((*config).*field); + if (!(*i)->fromMessage(msg, n)) + return false; + } + + return true; + } + + virtual void setInitialState(boost::any& cfg) const + { + PT* config = boost::any_cast(cfg); + T* group = &((*config).*field); + group->state = state; + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = boost::any(&((*config).*field)); + (*i)->setInitialState(n); + } + } + + virtual void updateParams(boost::any& cfg, PidConfig& top) const + { + PT* config = boost::any_cast(cfg); + + T* f = &((*config).*field); + f->setParams(top, abstract_parameters); + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + boost::any n = &((*config).*field); + (*i)->updateParams(n, top); + } + } + + virtual void toMessage(dynamic_reconfigure::Config& msg, const boost::any& cfg) const + { + const PT config = boost::any_cast(cfg); + dynamic_reconfigure::ConfigTools::appendGroup(msg, name, id, parent, config.*field); + + for (std::vector::const_iterator i = groups.begin(); i != groups.end(); ++i) + { + (*i)->toMessage(msg, config.*field); + } + } + + T(PT::*field); + std::vector groups; + }; + + class DEFAULT + { + public: + DEFAULT() + { + state = true; + name = "Default"; + } + + void setParams(PidConfig& config, const std::vector params) + { + for (std::vector::const_iterator _i = params.begin(); _i != params.end(); ++_i) + { + boost::any val; + (*_i)->getValue(config, val); + + if ("Kp_scale" == (*_i)->name) + { + Kp_scale = boost::any_cast(val); + } + if ("Kp" == (*_i)->name) + { + Kp = boost::any_cast(val); + } + if ("Ki_scale" == (*_i)->name) + { + Ki_scale = boost::any_cast(val); + } + if ("Ki" == (*_i)->name) + { + Ki = boost::any_cast(val); + } + if ("Kd_scale" == (*_i)->name) + { + Kd_scale = boost::any_cast(val); + } + if ("Kd" == (*_i)->name) + { + Kd = boost::any_cast(val); + } + } + } + + double Kp_scale; + double Kp; + double Ki_scale; + double Ki; + double Kd_scale; + double Kd; + + bool state; + std::string name; + + } groups; + + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kp_scale; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kp; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Ki_scale; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Ki; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kd_scale; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + double Kd; + //#line 218 + //"/opt/ros/indigo/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" + + bool __fromMessage__(dynamic_reconfigure::Config& msg) + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + const std::vector& __group_descriptions__ = __getGroupDescriptions__(); + + int count = 0; + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + if ((*i)->fromMessage(msg, *this)) + count++; + + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); i++) + { + if ((*i)->id == 0) + { + boost::any n = boost::any(this); + (*i)->updateParams(n, *this); + (*i)->fromMessage(msg, n); + } + } + + if (count != dynamic_reconfigure::ConfigTools::size(msg)) + { + ROS_ERROR("PidConfig::__fromMessage__ called with an unexpected parameter."); + ROS_ERROR("Booleans:"); + for (unsigned int i = 0; i < msg.bools.size(); i++) + ROS_ERROR(" %s", msg.bools[i].name.c_str()); + ROS_ERROR("Integers:"); + for (unsigned int i = 0; i < msg.ints.size(); i++) + ROS_ERROR(" %s", msg.ints[i].name.c_str()); + ROS_ERROR("Doubles:"); + for (unsigned int i = 0; i < msg.doubles.size(); i++) + ROS_ERROR(" %s", msg.doubles[i].name.c_str()); + ROS_ERROR("Strings:"); + for (unsigned int i = 0; i < msg.strs.size(); i++) + ROS_ERROR(" %s", msg.strs[i].name.c_str()); + // @todo Check that there are no duplicates. Make this error more + // explicit. + return false; + } + return true; + } + + // This version of __toMessage__ is used during initialization of + // statics when __getParamDescriptions__ can't be called yet. + void __toMessage__(dynamic_reconfigure::Config& msg, + const std::vector& __param_descriptions__, + const std::vector& __group_descriptions__) const + { + dynamic_reconfigure::ConfigTools::clear(msg); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->toMessage(msg, *this); + + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); ++i) + { + if ((*i)->id == 0) + { + (*i)->toMessage(msg, *this); + } + } + } + + void __toMessage__(dynamic_reconfigure::Config& msg) const + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + const std::vector& __group_descriptions__ = __getGroupDescriptions__(); + __toMessage__(msg, __param_descriptions__, __group_descriptions__); + } + + void __toServer__(const ros::NodeHandle& nh) const + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->toServer(nh, *this); + } + + void __fromServer__(const ros::NodeHandle& nh) + { + static bool setup = false; + + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->fromServer(nh, *this); + + const std::vector& __group_descriptions__ = __getGroupDescriptions__(); + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); i++) + { + if (!setup && (*i)->id == 0) + { + setup = true; + boost::any n = boost::any(this); + (*i)->setInitialState(n); + } + } + } + + void __clamp__() + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + const PidConfig& __max__ = __getMax__(); + const PidConfig& __min__ = __getMin__(); + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->clamp(*this, __max__, __min__); + } + + uint32_t __level__(const PidConfig& config) const + { + const std::vector& __param_descriptions__ = __getParamDescriptions__(); + uint32_t level = 0; + for (std::vector::const_iterator i = __param_descriptions__.begin(); + i != __param_descriptions__.end(); ++i) + (*i)->calcLevel(level, config, *this); + return level; + } + + static const dynamic_reconfigure::ConfigDescription& __getDescriptionMessage__(); + static const PidConfig& __getDefault__(); + static const PidConfig& __getMax__(); + static const PidConfig& __getMin__(); + static const std::vector& __getParamDescriptions__(); + static const std::vector& __getGroupDescriptions__(); + +private: + static const PidConfigStatics* __get_statics__(); +}; + +template <> // Max and min are ignored for strings. +inline void PidConfig::ParamDescription::clamp(PidConfig& config, const PidConfig& max, + const PidConfig& min) const +{ + return; +} + +class PidConfigStatics +{ + friend class PidConfig; + + PidConfigStatics() + { + PidConfig::GroupDescription Default("Default", "", 0, 0, true, &PidConfig::groups); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kp_scale = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kp_scale = 100.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kp_scale = 10.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp_scale", "double", 0, "Kp scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kp_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp_scale", "double", 0, "Kp scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kp_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kp = 0.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kp = 1.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kp = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp", "double", 0, "Kp", "", &PidConfig::Kp))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kp", "double", 0, "Kp", "", &PidConfig::Kp))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Ki_scale = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Ki_scale = 100.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Ki_scale = 10.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki_scale", "double", 0, "Ki scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Ki_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki_scale", "double", 0, "Ki scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Ki_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Ki = 0.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Ki = 1.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Ki = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki", "double", 0, "Ki", "", &PidConfig::Ki))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Ki", "double", 0, "Ki", "", &PidConfig::Ki))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kd_scale = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kd_scale = 100.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kd_scale = 10.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd_scale", "double", 0, "Kd scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kd_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd_scale", "double", 0, "Kd scale", + "{'enum_description': 'Scale factor for K setting', 'enum': " + "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 0.1, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_tenth'}, " + "{'srcline': 8, 'description': 'No scaling', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 1.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_unity'}, " + "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 10.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': " + "10, 'description': 'Scale by 100', 'srcfile': " + "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', " + "'cconsttype': 'const double', 'value': 100.0, 'ctype': " + "'double', 'type': 'double', 'name': 'scale_hundred'}]}", + &PidConfig::Kd_scale))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __min__.Kd = 0.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __max__.Kd = 1.0; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __default__.Kd = 0.1; + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.abstract_parameters.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd", "double", 0, "Kd", "", &PidConfig::Kd))); + //#line 262 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __param_descriptions__.push_back(PidConfig::AbstractParamDescriptionConstPtr( + new PidConfig::ParamDescription("Kd", "double", 0, "Kd", "", &PidConfig::Kd))); + //#line 233 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + Default.convertParams(); + //#line 233 + //"/opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator_catkin.py" + __group_descriptions__.push_back(PidConfig::AbstractGroupDescriptionConstPtr( + new PidConfig::GroupDescription(Default))); + //#line 353 + //"/opt/ros/indigo/share/dynamic_reconfigure/cmake/../templates/ConfigType.h.template" + + for (std::vector::const_iterator i = __group_descriptions__.begin(); + i != __group_descriptions__.end(); ++i) + { + __description_message__.groups.push_back(**i); + } + __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__); + __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__); + __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__); + } + std::vector __param_descriptions__; + std::vector __group_descriptions__; + PidConfig __max__; + PidConfig __min__; + PidConfig __default__; + dynamic_reconfigure::ConfigDescription __description_message__; + + static const PidConfigStatics* get_instance() + { + // Split this off in a separate function because I know that + // instance will get initialized the first time get_instance is + // called, and I am guaranteeing that get_instance gets called at + // most once. + static PidConfigStatics instance; + return &instance; + } +}; + +inline const dynamic_reconfigure::ConfigDescription& PidConfig::__getDescriptionMessage__() +{ + return __get_statics__()->__description_message__; +} + +inline const PidConfig& PidConfig::__getDefault__() +{ + return __get_statics__()->__default__; +} + +inline const PidConfig& PidConfig::__getMax__() +{ + return __get_statics__()->__max__; +} + +inline const PidConfig& PidConfig::__getMin__() +{ + return __get_statics__()->__min__; +} + +inline const std::vector& PidConfig::__getParamDescriptions__() +{ + return __get_statics__()->__param_descriptions__; +} + +inline const std::vector& PidConfig::__getGroupDescriptions__() +{ + return __get_statics__()->__group_descriptions__; +} + +inline const PidConfigStatics* PidConfig::__get_statics__() +{ + const static PidConfigStatics* statics; + + if (statics) // Common case + return statics; + + boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__); + + if (statics) // In case we lost a race. + return statics; + + statics = PidConfigStatics::get_instance(); + + return statics; +} + +//#line 7 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_tenth = 0.1; +//#line 8 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_unity = 1.0; +//#line 9 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_ten = 10.0; +//#line 10 "/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg" +const double Pid_scale_hundred = 100.0; +} + +#endif // __PIDRECONFIGURATOR_H__ diff --git a/ros/src/pid/include/pid/controller.h b/ros/src/pid/include/pid/controller.h new file mode 100644 index 0000000..397ddb5 --- /dev/null +++ b/ros/src/pid/include/pid/controller.h @@ -0,0 +1,112 @@ +/***************************************************************************/ /** + * \file controller.h + * + * \brief Simple PID controller with dynamic reconfigure + * \author Andy Zelenak + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#ifndef CONTROLLER_H +#define CONTROLLER_H + +namespace pid_ns +{ +class PidObject +{ +public: + PidObject(); + +private: + void doCalcs(); + void getParams(double in, double& value, double& scale); + void pidEnableCallback(const std_msgs::Bool& pid_enable_msg); + void plantStateCallback(const std_msgs::Float64& state_msg); + void printParameters(); + void reconfigureCallback(pid::PidConfig& config, uint32_t level); + void setpointCallback(const std_msgs::Float64& setpoint_msg); + bool validateParameters(); + + // Primary PID controller input & output variables + double plant_state_; // current output of plant + double control_effort_ = 0; // output of pid controller + double setpoint_ = 0; // desired output of plant + bool pid_enabled_ = true; // PID is enabled to run + bool new_state_or_setpt_ = false; // Indicate that fresh calculations need to be run + + ros::Time prev_time_; + ros::Duration delta_t_; + bool first_reconfig_ = true; + + double error_integral_ = 0; + double proportional_ = 0; // proportional term of output + double integral_ = 0; // integral term of output + double derivative_ = 0; // derivative term of output + + // PID gains + double Kp_ = 0, Ki_ = 0, Kd_ = 0; + + // Parameters for error calc. with disconinuous input + bool angle_error_ = false; + double angle_wrap_ = 2.0 * 3.14159; + + // Cutoff frequency for the derivative calculation in Hz. + // Negative -> Has not been set by the user yet, so use a default. + double cutoff_frequency_ = -1; + + // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency + // at + // 1/4 of the sample rate. + double c_ = 1.; + + // Used to check for tan(0)==>NaN in the filter calculation + double tan_filt_ = 1.; + + // Upper and lower saturation limits + double upper_limit_ = 1000, lower_limit_ = -1000; + + // Anti-windup term. Limits the absolute value of the integral term. + double windup_limit_ = 1000; + + // Initialize filter data with zeros + std::vector error_, filtered_error_, error_deriv_, filtered_error_deriv_; + + // Topic and node names and message objects + ros::Publisher control_effort_pub_; + std::string topic_from_controller_, topic_from_plant_, setpoint_topic_, pid_enable_topic_; + + std_msgs::Float64 control_msg_, state_msg_; + + // Diagnostic objects + double min_loop_frequency_ = 1, max_loop_frequency_ = 1000; + int measurements_received_ = 0; +}; +} // end pid namespace + +#endif diff --git a/ros/src/pid/include/pid/pid.h b/ros/src/pid/include/pid/pid.h new file mode 100644 index 0000000..720880f --- /dev/null +++ b/ros/src/pid/include/pid/pid.h @@ -0,0 +1,133 @@ +/***************************************************************************/ /** + * \file controller.h + * + * \brief Simple PID controller with dynamic reconfigure + * \author Andy Zelenak + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#ifndef PID_H +#define PID_H + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pid_ns +{ +class PidObject +{ +public: + PidObject(); + + // Primary output variable + double control_effort_ = 0; // output of pid controller + +private: + void doCalcs(); + void getParams(double in, double& value, double& scale); + void pidEnableCallback(const std_msgs::Bool& pid_enable_msg); + void plantStateCallback(const std_msgs::Float64& state_msg); + void printParameters(); + void reconfigureCallback(pid::PidConfig& config, uint32_t level); + void setpointCallback(const std_msgs::Float64& setpoint_msg); + bool validateParameters(); + + // Primary PID controller input variables + double plant_state_; // current output of plant + bool pid_enabled_ = true; // PID is enabled to run + bool new_state_or_setpt_ = false; // Indicate that fresh calculations need to be run + double setpoint_ = 0; // desired output of plant + + ros::Time prev_time_; + ros::Time last_setpoint_msg_time_; + ros::Duration delta_t_; + bool first_reconfig_ = true; + + double error_integral_ = 0; + double proportional_ = 0; // proportional term of output + double integral_ = 0; // integral term of output + double derivative_ = 0; // derivative term of output + + // PID gains + double Kp_ = 0, Ki_ = 0, Kd_ = 0; + + // Parameters for error calc. with disconinuous input + bool angle_error_ = false; + double angle_wrap_ = 2.0 * 3.14159; + + // Cutoff frequency for the derivative calculation in Hz. + // Negative -> Has not been set by the user yet, so use a default. + double cutoff_frequency_ = -1; + + // Setpoint timeout parameter to determine how long to keep publishing + // control_effort messages after last setpoint message + // -1 indicates publish indefinately, and positive number sets the timeout + double setpoint_timeout_ = -1; + + // Used in filter calculations. Default 1.0 corresponds to a cutoff frequency + // at + // 1/4 of the sample rate. + double c_ = 1.; + + // Used to check for tan(0)==>NaN in the filter calculation + double tan_filt_ = 1.; + + // Upper and lower saturation limits + double upper_limit_ = 1000, lower_limit_ = -1000; + + // Anti-windup term. Limits the absolute value of the integral term. + double windup_limit_ = 1000; + + // Initialize filter data with zeros + std::vector error_, filtered_error_, error_deriv_, filtered_error_deriv_; + + // Topic and node names and message objects + ros::Publisher control_effort_pub_; + ros::Publisher pid_debug_pub_; + + std::string topic_from_controller_, topic_from_plant_, setpoint_topic_, pid_enable_topic_; + std::string pid_debug_pub_name_; + std_msgs::Float64 control_msg_, state_msg_; + + // Diagnostic objects + double min_loop_frequency_ = 1, max_loop_frequency_ = 1000; + int measurements_received_ = 0; +}; +} // end pid namespace + +#endif diff --git a/ros/src/pid/launch/depth_control.launch b/ros/src/pid/launch/depth_control.launch new file mode 100644 index 0000000..cb612c1 --- /dev/null +++ b/ros/src/pid/launch/depth_control.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/pid/launch/differential_drive_sim.launch b/ros/src/pid/launch/differential_drive_sim.launch new file mode 100644 index 0000000..f14e2de --- /dev/null +++ b/ros/src/pid/launch/differential_drive_sim.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/pid/launch/servo_sim.launch b/ros/src/pid/launch/servo_sim.launch new file mode 100644 index 0000000..1c163b4 --- /dev/null +++ b/ros/src/pid/launch/servo_sim.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/pid/launch/sim_time.launch b/ros/src/pid/launch/sim_time.launch new file mode 100644 index 0000000..8c08b69 --- /dev/null +++ b/ros/src/pid/launch/sim_time.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/src/pid/license.txt b/ros/src/pid/license.txt new file mode 100644 index 0000000..e5b20d9 --- /dev/null +++ b/ros/src/pid/license.txt @@ -0,0 +1,12 @@ +Copyright (c) 2015, Andy Zelenak +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/ros/src/pid/package.xml b/ros/src/pid/package.xml new file mode 100644 index 0000000..81ac990 --- /dev/null +++ b/ros/src/pid/package.xml @@ -0,0 +1,25 @@ + + + pid + 0.0.28 + Launch a PID control node. + + Andy Zelenak + + BSD + + http://wiki.ros.org/pid + + Andy Zelenak + Paul Bouchier + + + catkin + + + message_runtime + roscpp + std_msgs + dynamic_reconfigure + + diff --git a/ros/src/pid/src/autotune.cpp b/ros/src/pid/src/autotune.cpp new file mode 100644 index 0000000..9d743e5 --- /dev/null +++ b/ros/src/pid/src/autotune.cpp @@ -0,0 +1,254 @@ +/***************************************************************************/ /** + * \file autotune.cpp + * + * \brief Autotune a PID controller with the Ziegler Nichols method + * \author Andy Zelenak + * \date October 25, 2016 + * + * \section license License (BSD-3) + * Copyright (c) 2016, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// Use the Ziegler Nichols Method to calculate reasonable PID gains. +// The ZN Method is based on setting Ki & Kd to zero, then cranking up Kp until +// oscillations are observed. +// This node varies Kp through a range until oscillations are just barely +// observed, +// then calculates the other parameters automatically. +// See https://en.wikipedia.org/wiki/PID_controller + +#include +#include +#include +#include + +// Use dynamic_reconfigure to adjust Kp, Ki, and Kd +#include +#include +#include + +void setKiKdToZero(); +void setKp(double Kp); +void setpointCallback(const std_msgs::Float64& setpoint_msg); +void stateCallback(const std_msgs::Float64& state_msg); +void setFinalParams(); + +namespace autotune +{ +double Ku = 0.; +double Tu = 0.; +double setpoint = 0.; +double state = 0.; +std::string ns = "/left_wheel_pid/"; +int oscillation_count = 0; +int num_loops = 100; // Will look for oscillations for num_loops*loopRate +int initial_error = 0; +double Kp_ZN = 0.; +double Ki_ZN = 0.; +double Kd_ZN = 0.; +bool found_Ku = false; +std::vector oscillation_times(3); // Used to calculate Tu, the oscillation period +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "autotune_node"); + ros::NodeHandle autotune_node; + ros::start(); + ros::Subscriber setpoint_sub = autotune_node.subscribe("/setpoint", 1, setpointCallback); + ros::Subscriber state_sub = autotune_node.subscribe("/state", 1, stateCallback); + ros::Rate loopRate(50); + + // Set Ki and Kd to zero for the ZN method with dynamic_reconfigure + setKiKdToZero(); + + // Define how rapidly the value of Kp is varied, and the max/min values to try + double Kp_max = 10.; + double Kp_min = 0.5; + double Kp_step = 1.0; + + for (double Kp = Kp_min; Kp <= Kp_max; Kp += Kp_step) + { + ////////////////////// + // Get the error sign. + ////////////////////// + // Need to wait for new setpoint/state msgs + ros::topic::waitForMessage("setpoint"); + ros::topic::waitForMessage("state"); + + // Try a new Kp. + setKp(Kp); + ROS_INFO_STREAM("Trying Kp = " << Kp); // Blank line on terminal + autotune::oscillation_count = 0; // Reset to look for oscillations again + + for (int i = 0; i < autotune::num_loops; i++) // Collect data for loopRate*num_loops seconds + { + ros::spinOnce(); + loopRate.sleep(); + if (i == 0) // Get the sign of the initial error + { + autotune::initial_error = (autotune::setpoint - autotune::state); + } + + // Did the system oscillate about the setpoint? If so, Kp~Ku. + // Oscillation => the sign of the error changes + // The first oscillation is caused by the change in setpoint. Ignore it. + // Look for 2 oscillations. + // Get a fresh state message + ros::topic::waitForMessage("state"); + double new_error = (autotune::setpoint - autotune::state); // Sign of the error + // ROS_INFO_STREAM("New error: "<< new_error); + if (std::signbit(autotune::initial_error) != std::signbit(new_error)) + { + autotune::oscillation_times.at(autotune::oscillation_count) = + loopRate.expectedCycleTime().toSec() * i; // Record the time to calculate a period, Tu + autotune::oscillation_count++; + ROS_INFO_STREAM("Oscillation occurred. Oscillation count: " << autotune::oscillation_count); + autotune::initial_error = new_error; // Reset to look for another oscillation + + // If the system is definitely oscillating about the setpoint + if (autotune::oscillation_count > 2) + { + // Now calculate the period of oscillation (Tu) + autotune::Tu = autotune::oscillation_times.at(2) - autotune::oscillation_times.at(0); + ROS_INFO_STREAM("Tu (oscillation period): " << autotune::Tu); + // ROS_INFO_STREAM( "2*sampling period: " << + // 2.*loopRate.expectedCycleTime().toSec() ); + + // We're looking for more than just the briefest dip across the + // setpoint and back. + // Want to see significant oscillation + if (autotune::Tu > 3. * loopRate.expectedCycleTime().toSec()) + { + autotune::Ku = Kp; + + // Now calculate the other parameters with ZN method + autotune::Kp_ZN = 0.6 * autotune::Ku; + autotune::Ki_ZN = 1.2 * autotune::Ku / autotune::Tu; + autotune::Kd_ZN = 3. * autotune::Ku * autotune::Tu / 40.; + + autotune::found_Ku = true; + goto DONE; + } + else + break; // Try the next Kp + } + } + } + } +DONE: + + if (autotune::found_Ku == true) + { + setFinalParams(); + } + else + ROS_INFO_STREAM("Did not see any oscillations for this range of Kp. Adjust " + "Kp_max and Kp_min to broaden the search."); + + ros::shutdown(); + return 0; +} + +// Set Ki and Kd to zero with dynamic_reconfigure +void setKiKdToZero() +{ + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::Config config; + double_param.name = "Ki"; + double_param.value = 0.0; + config.doubles.push_back(double_param); + double_param.name = "Kd"; + double_param.value = 0.0; + config.doubles.push_back(double_param); + srv_req.config = config; + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); +} + +// Set Kp with dynamic_reconfigure +void setKp(double Kp) +{ + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::Config config; + + // A blank service call to get the current parameters into srv_resp + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); + + double_param.name = "Kp"; + double_param.value = Kp / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + srv_req.config = config; + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); +} + +void setpointCallback(const std_msgs::Float64& setpoint_msg) +{ + autotune::setpoint = setpoint_msg.data; +} + +void stateCallback(const std_msgs::Float64& state_msg) +{ + autotune::state = state_msg.data; +} + +// Print out and set the final parameters as calculated by the autotuner +void setFinalParams() +{ + ROS_INFO_STREAM(" "); + ROS_INFO_STREAM("The suggested parameters are: "); + ROS_INFO_STREAM("Kp " << autotune::Kp_ZN); + ROS_INFO_STREAM("Ki " << autotune::Ki_ZN); + ROS_INFO_STREAM("Kd " << autotune::Kd_ZN); + + // Set the ZN parameters with dynamic_reconfigure + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::DoubleParameter double_param; + dynamic_reconfigure::Config config; + + // A blank service call to get the current parameters into srv_resp + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); + + double_param.name = "Kp"; + double_param.value = autotune::Kp_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + + double_param.name = "Ki"; + double_param.value = autotune::Ki_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + + double_param.name = "Kd"; + double_param.value = autotune::Kd_ZN / srv_resp.config.doubles.at(0).value; // Adjust for the scale slider on the GUI + config.doubles.push_back(double_param); + + srv_req.config = config; + ros::service::call(autotune::ns + "set_parameters", srv_req, srv_resp); +} \ No newline at end of file diff --git a/ros/src/pid/src/controller.cpp b/ros/src/pid/src/controller.cpp new file mode 100644 index 0000000..94581dc --- /dev/null +++ b/ros/src/pid/src/controller.cpp @@ -0,0 +1,50 @@ + +/***************************************************************************/ /** + * \file controller.cpp + * + * \brief Simple PID controller with dynamic reconfigure + * \author Andy Zelenak + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Andy Zelenak\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// Subscribe to a topic about the state of a dynamic system and calculate +// feedback to +// stabilize it. + +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "controller"); + + pid_ns::PidObject my_pid; + + return 0; +} diff --git a/ros/src/pid/src/pid.cpp b/ros/src/pid/src/pid.cpp new file mode 100644 index 0000000..1b0eebf --- /dev/null +++ b/ros/src/pid/src/pid.cpp @@ -0,0 +1,323 @@ + +#include + +using namespace pid_ns; + +PidObject::PidObject() : error_(3, 0), filtered_error_(3, 0), error_deriv_(3, 0), filtered_error_deriv_(3, 0) +{ + ros::NodeHandle node; + ros::NodeHandle node_priv("~"); + + while (ros::ok() && ros::Time(0) == ros::Time::now()) + { + ROS_INFO("controller spinning, waiting for time to become non-zero"); + sleep(1); + } + + // Get params if specified in launch file or as params on command-line, set + // defaults + node_priv.param("Kp", Kp_, 1.0); + node_priv.param("Ki", Ki_, 0.0); + node_priv.param("Kd", Kd_, 0.0); + node_priv.param("upper_limit", upper_limit_, 1000.0); + node_priv.param("lower_limit", lower_limit_, -1000.0); + node_priv.param("windup_limit", windup_limit_, 1000.0); + node_priv.param("cutoff_frequency", cutoff_frequency_, -1.0); + node_priv.param("topic_from_controller", topic_from_controller_, "control_effort"); + node_priv.param("topic_from_plant", topic_from_plant_, "state"); + node_priv.param("setpoint_topic", setpoint_topic_, "setpoint"); + node_priv.param("pid_enable_topic", pid_enable_topic_, "pid_enable"); + node_priv.param("max_loop_frequency", max_loop_frequency_, 1.0); + node_priv.param("min_loop_frequency", min_loop_frequency_, 1000.0); + node_priv.param("pid_debug_topic", pid_debug_pub_name_, "pid_debug"); + node_priv.param("setpoint_timeout", setpoint_timeout_, -1.0); + ROS_ASSERT_MSG(setpoint_timeout_ ==-1 || setpoint_timeout_ > 0, + "setpoint_timeout set to %.2f but needs to -1 or >0", setpoint_timeout_); + + // Two parameters to allow for error calculation with discontinous value + node_priv.param("angle_error", angle_error_, false); + node_priv.param("angle_wrap", angle_wrap_, 2.0 * 3.14159); + + // Update params if specified as command-line options, & print settings + printParameters(); + if (not validateParameters()) + std::cout << "Error: invalid parameter\n"; + + // instantiate publishers & subscribers + control_effort_pub_ = node.advertise(topic_from_controller_, 1); + pid_debug_pub_ = node.advertise(pid_debug_pub_name_, 1); + + ros::Subscriber plant_sub_ = node.subscribe(topic_from_plant_, 1, &PidObject::plantStateCallback, this); + ros::Subscriber setpoint_sub_ = node.subscribe(setpoint_topic_, 1, &PidObject::setpointCallback, this); + ros::Subscriber pid_enabled_sub_ = node.subscribe(pid_enable_topic_, 1, &PidObject::pidEnableCallback, this); + + if (!plant_sub_ || !setpoint_sub_ || !pid_enabled_sub_) + { + ROS_ERROR_STREAM("Initialization of a subscriber failed. Exiting."); + ros::shutdown(); + exit(EXIT_FAILURE); + } + + // dynamic reconfiguration + dynamic_reconfigure::Server config_server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&PidObject::reconfigureCallback, this, _1, _2); + config_server.setCallback(f); + + // Wait for first messages + while( ros::ok() && !ros::topic::waitForMessage(setpoint_topic_, ros::Duration(10.))) + ROS_WARN_STREAM("Waiting for first setpoint message."); + + while( ros::ok() && !ros::topic::waitForMessage(topic_from_plant_, ros::Duration(10.))) + ROS_WARN_STREAM("Waiting for first state message from the plant."); + + // Respond to inputs until shut down + while (ros::ok()) + { + doCalcs(); + ros::spinOnce(); + + // Add a small sleep to avoid 100% CPU usage + ros::Duration(0.001).sleep(); + } +}; + +void PidObject::setpointCallback(const std_msgs::Float64& setpoint_msg) +{ + setpoint_ = setpoint_msg.data; + last_setpoint_msg_time_ = ros::Time::now(); + new_state_or_setpt_ = true; +} + +void PidObject::plantStateCallback(const std_msgs::Float64& state_msg) +{ + plant_state_ = state_msg.data; + + new_state_or_setpt_ = true; +} + +void PidObject::pidEnableCallback(const std_msgs::Bool& pid_enable_msg) +{ + pid_enabled_ = pid_enable_msg.data; +} + +void PidObject::getParams(double in, double& value, double& scale) +{ + int digits = 0; + value = in; + while (ros::ok() && ((fabs(value) > 1.0 || fabs(value) < 0.1) && (digits < 2 && digits > -1))) + { + if (fabs(value) > 1.0) + { + value /= 10.0; + digits++; + } + else + { + value *= 10.0; + digits--; + } + } + if (value > 1.0) + value = 1.0; + if (value < -1.0) + value = -1.0; + + scale = pow(10.0, digits); +} + +bool PidObject::validateParameters() +{ + if (lower_limit_ > upper_limit_) + { + ROS_ERROR("The lower saturation limit cannot be greater than the upper " + "saturation limit."); + return (false); + } + + return true; +} + +void PidObject::printParameters() +{ + std::cout << std::endl << "PID PARAMETERS" << std::endl << "-----------------------------------------" << std::endl; + std::cout << "Kp: " << Kp_ << ", Ki: " << Ki_ << ", Kd: " << Kd_ << std::endl; + if (cutoff_frequency_ == -1) // If the cutoff frequency was not specified by the user + std::cout << "LPF cutoff frequency: 1/4 of sampling rate" << std::endl; + else + std::cout << "LPF cutoff frequency: " << cutoff_frequency_ << std::endl; + std::cout << "pid node name: " << ros::this_node::getName() << std::endl; + std::cout << "Name of topic from controller: " << topic_from_controller_ << std::endl; + std::cout << "Name of topic from the plant: " << topic_from_plant_ << std::endl; + std::cout << "Name of setpoint topic: " << setpoint_topic_ << std::endl; + std::cout << "Integral-windup limit: " << windup_limit_ << std::endl; + std::cout << "Saturation limits: " << upper_limit_ << "/" << lower_limit_ << std::endl; + std::cout << "-----------------------------------------" << std::endl; + + return; +} + +void PidObject::reconfigureCallback(pid::PidConfig& config, uint32_t level) +{ + if (first_reconfig_) + { + getParams(Kp_, config.Kp, config.Kp_scale); + getParams(Ki_, config.Ki, config.Ki_scale); + getParams(Kd_, config.Kd, config.Kd_scale); + first_reconfig_ = false; + return; // Ignore the first call to reconfigure which happens at startup + } + + Kp_ = config.Kp * config.Kp_scale; + Ki_ = config.Ki * config.Ki_scale; + Kd_ = config.Kd * config.Kd_scale; + ROS_INFO("Pid reconfigure request: Kp: %f, Ki: %f, Kd: %f", Kp_, Ki_, Kd_); +} + +void PidObject::doCalcs() +{ + // Do fresh calcs if knowledge of the system has changed. + if (new_state_or_setpt_) + { + if (!((Kp_ <= 0. && Ki_ <= 0. && Kd_ <= 0.) || + (Kp_ >= 0. && Ki_ >= 0. && Kd_ >= 0.))) // All 3 gains should have the same sign + ROS_WARN("All three gains (Kp, Ki, Kd) should have the same sign for " + "stability."); + + error_.at(2) = error_.at(1); + error_.at(1) = error_.at(0); + error_.at(0) = setpoint_ - plant_state_; // Current error goes to slot 0 + + // If the angle_error param is true, then address discontinuity in error + // calc. + // For example, this maintains an angular error between -180:180. + if (angle_error_) + { + while (error_.at(0) < -1.0 * angle_wrap_ / 2.0) + { + error_.at(0) += angle_wrap_; + + // The proportional error will flip sign, but the integral error + // won't and the filtered derivative will be poorly defined. So, + // reset them. + error_deriv_.at(2) = 0.; + error_deriv_.at(1) = 0.; + error_deriv_.at(0) = 0.; + error_integral_ = 0.; + } + while (error_.at(0) > angle_wrap_ / 2.0) + { + error_.at(0) -= angle_wrap_; + + // The proportional error will flip sign, but the integral error + // won't and the filtered derivative will be poorly defined. So, + // reset them. + error_deriv_.at(2) = 0.; + error_deriv_.at(1) = 0.; + error_deriv_.at(0) = 0.; + error_integral_ = 0.; + } + } + + // calculate delta_t + if (!prev_time_.isZero()) // Not first time through the program + { + delta_t_ = ros::Time::now() - prev_time_; + prev_time_ = ros::Time::now(); + if (0 == delta_t_.toSec()) + { + ROS_ERROR("delta_t is 0, skipping this loop. Possible overloaded cpu " + "at time: %f", + ros::Time::now().toSec()); + return; + } + } + else + { + ROS_INFO("prev_time is 0, doing nothing"); + prev_time_ = ros::Time::now(); + return; + } + + // integrate the error + error_integral_ += error_.at(0) * delta_t_.toSec(); + + // Apply windup limit to limit the size of the integral term + if (error_integral_ > fabsf(windup_limit_)) + error_integral_ = fabsf(windup_limit_); + + if (error_integral_ < -fabsf(windup_limit_)) + error_integral_ = -fabsf(windup_limit_); + + // My filter reference was Julius O. Smith III, Intro. to Digital Filters + // With Audio Applications. + // See https://ccrma.stanford.edu/~jos/filters/Example_Second_Order_Butterworth_Lowpass.html + if (cutoff_frequency_ != -1) + { + // Check if tan(_) is really small, could cause c = NaN + tan_filt_ = tan((cutoff_frequency_ * 6.2832) * delta_t_.toSec() / 2); + + // Avoid tan(0) ==> NaN + if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) + tan_filt_ = -0.01; + if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) + tan_filt_ = 0.01; + + c_ = 1 / tan_filt_; + } + + filtered_error_.at(2) = filtered_error_.at(1); + filtered_error_.at(1) = filtered_error_.at(0); + filtered_error_.at(0) = (1 / (1 + c_ * c_ + 1.414 * c_)) * (error_.at(2) + 2 * error_.at(1) + error_.at(0) - + (c_ * c_ - 1.414 * c_ + 1) * filtered_error_.at(2) - + (-2 * c_ * c_ + 2) * filtered_error_.at(1)); + + // Take derivative of error + // First the raw, unfiltered data: + error_deriv_.at(2) = error_deriv_.at(1); + error_deriv_.at(1) = error_deriv_.at(0); + error_deriv_.at(0) = (error_.at(0) - error_.at(1)) / delta_t_.toSec(); + + filtered_error_deriv_.at(2) = filtered_error_deriv_.at(1); + filtered_error_deriv_.at(1) = filtered_error_deriv_.at(0); + + filtered_error_deriv_.at(0) = + (1 / (1 + c_ * c_ + 1.414 * c_)) * + (error_deriv_.at(2) + 2 * error_deriv_.at(1) + error_deriv_.at(0) - + (c_ * c_ - 1.414 * c_ + 1) * filtered_error_deriv_.at(2) - (-2 * c_ * c_ + 2) * filtered_error_deriv_.at(1)); + + // calculate the control effort + proportional_ = Kp_ * filtered_error_.at(0); + integral_ = Ki_ * error_integral_; + derivative_ = Kd_ * filtered_error_deriv_.at(0); + control_effort_ = proportional_ + integral_ + derivative_; + + // Apply saturation limits + if (control_effort_ > upper_limit_) + control_effort_ = upper_limit_; + else if (control_effort_ < lower_limit_) + control_effort_ = lower_limit_; + + // Publish the stabilizing control effort if the controller is enabled + if (pid_enabled_ && (setpoint_timeout_ == -1 || + (ros::Time::now() - last_setpoint_msg_time_).toSec() <= setpoint_timeout_)) + { + control_msg_.data = control_effort_; + control_effort_pub_.publish(control_msg_); + // Publish topic with + std::vector pid_debug_vect { plant_state_, control_effort_, proportional_, integral_, derivative_}; + std_msgs::Float64MultiArray pidDebugMsg; + pidDebugMsg.data = pid_debug_vect; + pid_debug_pub_.publish(pidDebugMsg); + } + else if (setpoint_timeout_ > 0 && (ros::Time::now() - last_setpoint_msg_time_).toSec() > setpoint_timeout_) + { + ROS_WARN_ONCE("Setpoint message timed out, will stop publising control_effort_messages"); + error_integral_ = 0.0; + } + else + error_integral_ = 0.0; + } + + new_state_or_setpt_ = false; +} diff --git a/ros/src/pid/src/plant_sim.cpp b/ros/src/pid/src/plant_sim.cpp new file mode 100644 index 0000000..f43e843 --- /dev/null +++ b/ros/src/pid/src/plant_sim.cpp @@ -0,0 +1,173 @@ +/***************************************************************************/ /** + * \file plant_sim.h + * + * \brief First or second order plant simulator + * \author Andy Zelenak + * \author Paul Bouchier + * \date March 8, 2015 + * + * \section license License (BSD-3) + * Copyright (c) 2015, Paul Bouchier\n + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +// This file simulates a 1st or 2nd-order dynamic system, publishes its state, +// and subscribes to a 'control_effort' topic. The control effort is used +// to stabilize the servo. + +#include +#include + +namespace plant_sim +{ +// Global so it can be passed from the callback fxn to main +static double control_effort = 0.0; +static bool reverse_acting = false; +} +using namespace plant_sim; + +// Callback when something is published on 'control_effort' +void controlEffortCallback(const std_msgs::Float64& control_effort_input) +{ + // the stabilizing control effort + if (reverse_acting) + { + control_effort = -control_effort_input.data; + } + else + { + control_effort = control_effort_input.data; + } +} + +int main(int argc, char** argv) +{ + int plant_order = 1; + double temp = 4.7; // initial condition for first-order plant + double displacement = 3.3; // initial condition for second-order plant + + ros::init(argc, argv, "plant"); + ros::NodeHandle sim_node; + + while (ros::ok() && ros::Time(0) == ros::Time::now()) + { + ROS_INFO("Plant_sim spinning waiting for time to become non-zero"); + sleep(1); + } + + ros::NodeHandle node_priv("~"); + node_priv.param("plant_order", plant_order, 1); + node_priv.param("reverse_acting", reverse_acting, false); + + if (plant_order == 1) + { + ROS_INFO("Starting simulation of a first-order plant."); + } + else if (plant_order == 2) + { + ROS_INFO("Starting simulation of a second-order plant."); + } + else + { + ROS_ERROR("Error: Invalid plant type parameter, must be 1 or 2: %s", argv[1]); + return -1; + } + + // Advertise a plant state msg + std_msgs::Float64 plant_state; + ros::Publisher servo_state_pub = sim_node.advertise("state", 1); + + // Subscribe to "control_effort" topic to get a controller_msg.msg + ros::Subscriber sub = sim_node.subscribe("control_effort", 1, controlEffortCallback); + + int loop_counter = 0; + double delta_t = 0.01; + ros::Rate loop_rate(1 / delta_t); // Control rate in Hz + + // Initialize 1st-order (e.g temp controller) process variables + double temp_rate = 0; // rate of temp change + + // Initialize 2nd-order (e.g. servo-motor with load) process variables + double speed = 0; // meters/sec + double acceleration = 0; // meters/sec^2 + double mass = 0.1; // in kg + double friction = 1.0; // a decelerating force factor + double stiction = 1; // control_effort must exceed this before stationary servo moves + double Kv = 1; // motor constant: force (newtons) / volt + double Kbackemf = 0; // Volts of back-emf per meter/sec of speed + double decel_force; // decelerating force + + while (ros::ok()) + { + ros::spinOnce(); + + switch (plant_order) + { + case 1: // First order plant + temp_rate = (0.1 * temp) + control_effort; + temp = temp + temp_rate * delta_t; + + plant_state.data = temp; + break; + + case 2: // Second order plant + if (fabs(speed) < 0.001) + { + // if nearly stopped, stop it & require overcoming stiction to restart + speed = 0; + if (fabs(control_effort) < stiction) + { + control_effort = 0; + } + } + + // Update the servo. + // control_effort: the voltage applied to the servo. Output from PID + // controller. It is + // opposed by back emf (expressed as speed) to produce a net force. + // Range: -1 to +1 + // displacement: the actual value of the servo output position. Input to + // PID controller + + decel_force = -(speed * friction); // can be +ve or -ve. Linear with speed + acceleration = ((Kv * (control_effort - (Kbackemf * speed)) + decel_force) / mass); // a = F/m + speed = speed + (acceleration * delta_t); + displacement = displacement + speed * delta_t; + + plant_state.data = displacement; + break; + + default: + ROS_ERROR("Invalid plant_order: %d", plant_order); + return (-1); + } + + servo_state_pub.publish(plant_state); + loop_rate.sleep(); + } + + return 0; +} diff --git a/ros/src/pid/src/setpoint_node.cpp b/ros/src/pid/src/setpoint_node.cpp new file mode 100644 index 0000000..f5c0c16 --- /dev/null +++ b/ros/src/pid/src/setpoint_node.cpp @@ -0,0 +1,67 @@ +/***************************************************************************/ /** + * \file setpoint_node.cpp + * + * \brief Node that publishes time-varying setpoint values + * \author Paul Bouchier + * \date January 9, 2016 + * + * \section license License (BSD-3) + * Copyright (c) 2016, Paul Bouchier + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include "ros/ros.h" +#include "std_msgs/Float64.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "setpoint_node"); + ROS_INFO("Starting setpoint publisher"); + ros::NodeHandle setpoint_node; + + while (ros::ok() && ros::Time(0) == ros::Time::now()) + { + ROS_INFO("Setpoint_node spinning, waiting for time to become non-zero"); + sleep(1); + } + + std_msgs::Float64 setpoint; + setpoint.data = 1.0; + ros::Publisher setpoint_pub = setpoint_node.advertise("setpoint", 1); + + ros::Rate loop_rate(0.2); // change setpoint every 5 seconds + + while (ros::ok()) + { + ros::spinOnce(); + + setpoint_pub.publish(setpoint); // publish twice so graph gets it as a step + setpoint.data = 0 - setpoint.data; + setpoint_pub.publish(setpoint); + + loop_rate.sleep(); + } +} diff --git a/ros/src/pid/src/sim_time.cpp b/ros/src/pid/src/sim_time.cpp new file mode 100644 index 0000000..2cd6576 --- /dev/null +++ b/ros/src/pid/src/sim_time.cpp @@ -0,0 +1,94 @@ +/***************************************************************************/ /** + * \file sim_time.cpp + * + * \brief Node that publishes simulated time to the /clock topic + * + * \author Paul Bouchier + * \date January 27, 2016 + * + * \section license License (BSD-3) + * Copyright (c) 2016, Paul Bouchier + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Willow Garage, Inc. nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include "ros/ros.h" +#include "rosgraph_msgs/Clock.h" + +#include + +#define SIM_TIME_INCREMENT_US 10000 + +/* + * This node publishes increments of 1ms in time to the /clock topic. It does so + * at a rate determined by sim_speedup (simulation speedup factor), which should + * be passed + * in as a private parameter. + */ + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sim_time_source"); + ros::NodeHandle sim_time_node; + + // support integral multiples of wallclock time for simulation speedup + int sim_speedup; // integral factor by which to speed up simulation + ros::NodeHandle node_priv("~"); + node_priv.param("sim_speedup", sim_speedup, 1); + + // get the current time & populate sim_time with it + struct timeval start; + int rv = gettimeofday(&start, NULL); + usleep(1000); + struct timeval now; + rv = gettimeofday(&now, NULL); + if (0 != rv) + { + ROS_ERROR("Invalid return from gettimeofday: %d", rv); + return -1; + } + + rosgraph_msgs::Clock sim_time; + sim_time.clock.sec = now.tv_sec - start.tv_sec; + sim_time.clock.nsec = now.tv_usec * 1000; + ros::Publisher sim_time_pub = sim_time_node.advertise("clock", 1); + + ROS_INFO("Starting simulation time publisher at time: %d.%d", sim_time.clock.sec, sim_time.clock.nsec); + + while (ros::ok()) + { + sim_time_pub.publish(sim_time); + + sim_time.clock.nsec = sim_time.clock.nsec + SIM_TIME_INCREMENT_US * 1000; + while (sim_time.clock.nsec > 1000000000) + { + sim_time.clock.nsec -= 1000000000; + ++sim_time.clock.sec; + } + + usleep(SIM_TIME_INCREMENT_US / sim_speedup); + ros::spinOnce(); + } +} \ No newline at end of file diff --git a/ros/src/roslib_comm/package.xml b/ros/src/roslib_comm/package.xml deleted file mode 100644 index d04dd05..0000000 --- a/ros/src/roslib_comm/package.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - roslib_comm - 0.0.0 - The roslib_comm package - - - - - eric - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - sensor_msgs - std_msgs - shared_msgs - roscpp - rospy - sensor_msgs - std_msgs - shared_msgs - roscpp - rospy - sensor_msgs - std_msgs - shared_msgs - - - - - - - - diff --git a/ros/src/roslib_comm/scripts/gamepad_listener.py b/ros/src/roslib_comm/scripts/gamepad_listener.py deleted file mode 100755 index 507ccd2..0000000 --- a/ros/src/roslib_comm/scripts/gamepad_listener.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python3 -import rospy -from std_msgs.msg import String -from geometry_msgs.msg import Twist -from shared_msgs.msg import controller_msg -import json - -pub = rospy.Publisher('/gamepad_listener', controller_msg, queue_size=10) - -''' -class Callback(): - def __init__(self): - self.pub = rospy.Publisher('/gamepad_listener', controller_msg, queue_size=10) - - def callback(self, data): - stuff = json.loads(data.data) - msg = controller_msg() - msg.RX_axis = stuff['RSX'] - msg.RY_axis = stuff['RSY'] - msg.LX_axis = stuff['LSX'] - msg.LY_axis = stuff['LSY'] - msg.a = stuff['A'] - msg.b = stuff['B'] - msg.x = stuff['X'] - msg.y = stuff['Y'] - msg.DPadX = stuff['DPADX'] - msg.DPadY = stuff['DPADY'] - msg.RB = stuff['RB'] - msg.LB = stuff['LB'] - msg.Rtrigger = stuff['RT'] - msg.Ltrigger = stuff['LT'] - rospy.loginfo(msg) - self.pub.publish(msg) - -''' - - -def callback(data): - stuff = json.loads(data.data) - msg = controller_msg() - msg.RX_axis = stuff['RSX'] - msg.RY_axis = stuff['RSY'] - msg.LX_axis = stuff['LSX'] - msg.LY_axis = stuff['LSY'] - msg.a = stuff['A'] - msg.b = stuff['B'] - msg.x = stuff['X'] - msg.y = stuff['Y'] - msg.DPadX = stuff['DPADX'] - msg.DPadY = stuff['DPADY'] - msg.RB = stuff['RB'] - msg.LB = stuff['LB'] - msg.Rtrigger = stuff['RT'] - msg.Ltrigger = stuff['LT'] - rospy.loginfo(msg) - pub.publish(msg) - - -def listener(): - rospy.init_node('gp_listen', anonymous=True) - - rospy.Subscriber('chatter', String, callback) - - rospy.spin() - - -if __name__ == '__main__': - try: - listener() - except rospy.ROSInterruptException: - pass diff --git a/ros/src/roslib_comm/scripts/test_dummy.py b/ros/src/roslib_comm/scripts/test_dummy.py deleted file mode 100644 index bc3b642..0000000 --- a/ros/src/roslib_comm/scripts/test_dummy.py +++ /dev/null @@ -1,25 +0,0 @@ -#!/usr/bin/env python - -import rospy -from geometry_msgs.msg import Twist -from shared_msgs.msg import imu_msg - -def talker(): - rospy.init_node('test_dummy', anonymous=True) - pub = rospy.Publisher('/imu', Twist, queue_size=10) - rate = rospy.Rate(10) - - while not rospy.is_shutdown(): - msg = Twist() - msg.linear.x = 0 - msg.linear.y = 1 - msg.linear.z = -1 - msg.angular.x = 0 - msg.angular.y = 1 - msg.angular.z = -1 - rospy.loginfo(msg) - pub.publish(msg) - rate.sleep() - -if __name__ == '__main__': - talker() \ No newline at end of file diff --git a/ros/src/roslib_comm/scripts/upstream_sender.py b/ros/src/roslib_comm/scripts/upstream_sender.py deleted file mode 100644 index a5f0bf9..0000000 --- a/ros/src/roslib_comm/scripts/upstream_sender.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env python3 -# license removed for brevity -import rospy -from std_msgs.msg import String -from geometry_msgs.msg import Twist - -def talker(): - pub = rospy.Publisher('/upstream_data', Twist, queue_size=10) - rospy.init_node('upstream_sender', anonymous=True) - rate = rospy.Rate(10) # 10hz - while not rospy.is_shutdown(): - imu = Twist() - imu.linear.x = 1 - imu.linear.y = -1 - imu.linear.z = 0 - imu.angular.x = 1 - imu.angular.y = -1 - imu.angular.z = 0 - rospy.loginfo(imu) - pub.publish(imu) - rate.sleep() - -if __name__ == '__main__': - try: - talker() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/ros/src/shared_msgs/CMakeLists.txt b/ros/src/shared_msgs/CMakeLists.txt index d574296..a258121 100644 --- a/ros/src/shared_msgs/CMakeLists.txt +++ b/ros/src/shared_msgs/CMakeLists.txt @@ -48,6 +48,7 @@ add_message_files( esc_single_msg.msg auto_control_msg.msg auto_command_msg.msg + esc_test_data_msg.msg pressure_msg.msg thrust_command_msg.msg final_thrust_msg.msg @@ -62,6 +63,7 @@ add_message_files( thrust_disable_inverted_msg.msg rov_velocity_command.msg servo_msg.msg + com_msg.msg ) ## Generate services in the 'srv' folder diff --git a/ros/src/shared_msgs/msg/com_msg.msg b/ros/src/shared_msgs/msg/com_msg.msg new file mode 100644 index 0000000..92b5de2 --- /dev/null +++ b/ros/src/shared_msgs/msg/com_msg.msg @@ -0,0 +1 @@ +float32[3] com \ No newline at end of file diff --git a/ros/src/shared_msgs/msg/esc_test_data_msg.msg b/ros/src/shared_msgs/msg/esc_test_data_msg.msg new file mode 100644 index 0000000..d5313c9 --- /dev/null +++ b/ros/src/shared_msgs/msg/esc_test_data_msg.msg @@ -0,0 +1,6 @@ +int8 escNum +int8 temperature +float32 voltage +float32 current +string energy +string speed diff --git a/ros/src/shared_msgs/msg/rov_velocity_command.msg b/ros/src/shared_msgs/msg/rov_velocity_command.msg index 8a2bff8..fd7ecaf 100644 --- a/ros/src/shared_msgs/msg/rov_velocity_command.msg +++ b/ros/src/shared_msgs/msg/rov_velocity_command.msg @@ -1,4 +1,6 @@ geometry_msgs/Twist twist string source +bool isFine +float32 multiplier bool isPercentPower bool isPoolCentric \ No newline at end of file diff --git a/ros/src/shared_msgs/msg/thrust_command_msg.msg b/ros/src/shared_msgs/msg/thrust_command_msg.msg index 7a1d3b4..0fc823f 100644 --- a/ros/src/shared_msgs/msg/thrust_command_msg.msg +++ b/ros/src/shared_msgs/msg/thrust_command_msg.msg @@ -1,3 +1,4 @@ float32[6] desired_thrust - +bool isFine +float32 multiplier diff --git a/ros/src/shared_msgs/msg/tools_command_msg.msg b/ros/src/shared_msgs/msg/tools_command_msg.msg index ef7f182..cde26ae 100644 --- a/ros/src/shared_msgs/msg/tools_command_msg.msg +++ b/ros/src/shared_msgs/msg/tools_command_msg.msg @@ -1,4 +1,2 @@ -int8 pm -int8 secondary -int8 ghost +int8[4] tools diff --git a/ros/src/i2c_com/CMakeLists.txt b/ros/src/utils/CMakeLists.txt similarity index 95% rename from ros/src/i2c_com/CMakeLists.txt rename to ros/src/utils/CMakeLists.txt index 7e1df39..7be3c67 100644 --- a/ros/src/i2c_com/CMakeLists.txt +++ b/ros/src/utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(i2c_com) +project(utils) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -7,7 +7,7 @@ project(i2c_com) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED shared_msgs) +find_package(catkin REQUIRED COMPONENTS shared_msgs rospy) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -100,8 +100,8 @@ find_package(catkin REQUIRED shared_msgs) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES i2c_com -# CATKIN_DEPENDS other_catkin_pkg +# LIBRARIES can_com + CATKIN_DEPENDS # DEPENDS system_lib ) @@ -118,7 +118,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/i2c_com.cpp +# src/${PROJECT_NAME}/can_com.cpp # ) ## Add cmake target dependencies of the library @@ -129,7 +129,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/i2c_com_node.cpp) +# add_executable(${PROJECT_NAME}_node src/can_com_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -186,7 +186,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_i2c_com.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_can_com.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/ros/src/utils/README.md b/ros/src/utils/README.md new file mode 100644 index 0000000..6c88345 --- /dev/null +++ b/ros/src/utils/README.md @@ -0,0 +1,7 @@ +# can_receive_test.py +# esc_1by1_test.py +# motor_drive_intf.py +# solenoid_test.py +# solenoid_test_all.py +# thrust_brown_test_grant.py +# thrust_range_test.py diff --git a/ros/src/utils/package.xml b/ros/src/utils/package.xml new file mode 100644 index 0000000..c180758 --- /dev/null +++ b/ros/src/utils/package.xml @@ -0,0 +1,36 @@ + + + utils + 0.0.0 + The utils package + + + + + keith + + + + + + TODO + + catkin + rospy + shared_msgs + std_msgs + rospy + sensor_msgs + shared_msgs + std_msgs + rospy + shared_msgs + std_msgs + + + + + + + + diff --git a/ros/src/utils/src/ESC_Status.py b/ros/src/utils/src/ESC_Status.py new file mode 100755 index 0000000..000621d --- /dev/null +++ b/ros/src/utils/src/ESC_Status.py @@ -0,0 +1,49 @@ +#! /usr/bin/python3 + +import sys + +import can + +import rospy +from shared_msgs.msg import esc_test_data_msg as Data + +lastEsc = [0] * 3 + +def sendEscData(can_bus): + pub = rospy.Publisher('rov/ESCdata', Data, queue_size=10) # Modify queue size based on rate + rospy.init_node('ESC_Logger', anonymous=True) + rate = rospy.Rate(1000) # Figure out what rate to use + + while not rospy.is_shutdown(): + for can_rx in can_bus: + if rospy.is_shutdown(): + break + can_id = can_rx.arbitration_id + if (0x301 <= can_id <= 0x303): + lastEsc_ = lastEsc[can_id - 0x301] + lastEsc[can_id - 0x301] = (lastEsc_ + 1) % 4 + data = list(can_rx.data) # Get data from ESC + msg = Data() + msg.escNum = lastEsc_ # Add ESC Number + msg.temperature = data[0] # Add temperature data (deg C) + msg.voltage = data[1] / 255 + 10 # Add voltage data (V) + msg.current = (data[2] << 8 + data[3]) / 100 # Add current data (A) + msg.energy = (data[4] << 8 + data[5]) # Add energy data (mAHr) + msg.speed = (data[6] << 8 + data[7]) * 100 # Add speed data (erpm) + + # Send information from node + rospy.loginfo(msg) + pub.publish(msg) + rate.sleep() + +def main(args: list) -> None: + """""" + channel = "can0" + can_bus = can.interface.Bus(channel = channel, bustype = 'socketcan') + sendEscData(can_bus) + +if(__name__ == "__main__"): + try: + main(sys.argv) + except ROSInterruptException: + pass diff --git a/ros/src/utils/src/can_receive_test.py b/ros/src/utils/src/can_receive_test.py new file mode 100755 index 0000000..507aeee --- /dev/null +++ b/ros/src/utils/src/can_receive_test.py @@ -0,0 +1,43 @@ +#! /usr/bin/python3 +""" +This file prints out all incoming CAN messages. FUN! +""" + + +import sys + +import can + +lastEsc = [0] * 3 + + +def bus_message_received(can_rx: "can.Message") -> None: + can_id = can_rx.arbitration_id + if(0x301 <= can_id <= 0x303): + lastEsc_ = lastEsc[can_id - 0x301] + lastEsc[can_id - 0x301] = (lastEsc_ + 1) % 4 + data = list(can_rx.data) + print(f"ESC # {lastEsc_}") + print(f"Temperature {data[0]} C.") + print(f"Voltage {data[1] / 255 + 10} V.") + print(f"Current {(data[2] << 8 + data[3]) / 100} A.") + print(f"Energy {(data[4] << 8 + data[5])} mAHr.") + print(f"Speed {(data[6] << 8 + data[7]) * 100} erpm.\n") + else: + print(type(can_rx)) + print(f"data: {list(can_rx.data)}") + print(f"id: {can_id:03X}") + print(f"message {can_rx}\n\n") + + +def main(args: list) -> None: + """""" + channel = "can0" + can_bus = can.interface.Bus(channel = channel, bustype = 'socketcan') + + for can_rx in can_bus: + bus_message_received(can_rx) + + +if(__name__ == "__main__"): + main(sys.argv) diff --git a/ros/src/utils/src/esc_1by1_test.py b/ros/src/utils/src/esc_1by1_test.py new file mode 100644 index 0000000..9678f75 --- /dev/null +++ b/ros/src/utils/src/esc_1by1_test.py @@ -0,0 +1,94 @@ +#! /usr/bin/python3 +""" +This file is made for checking the functionality of each thruster. It goes over +every thruster of every ESC one by one. +""" +import argparse +import sys +import time +from copy import deepcopy + +import can +import signal + +from thrust_range_test import getSignal, writeToCan, N_CAN_BYTES, ZERO_THROTTLE +# from ...can_proc.scripts.thrust_proc import can_better_map + +can_better_map = { + 0x201: [ 1, 2, 3, 4 ], + 0x202: [ 5, 6, 7, 8 ], + 0x203: [ 0, 0, 0, 0 ] +} + +DELAY = 3 +N_THRUSTERS = 8 +LIL_FORWARD = ZERO_THROTTLE + 13 +HALT_BYTE_ARRAY = bytearray([ZERO_THROTTLE] * N_CAN_BYTES) + +BASE_PACKET = { + 0x201: [ZERO_THROTTLE] * N_CAN_BYTES, + 0x202: [ZERO_THROTTLE] * N_CAN_BYTES, + 0x203: [ZERO_THROTTLE] * N_CAN_BYTES +} + + +def main(args: list) -> None: + """""" + parser = argparse.ArgumentParser(description = __doc__) + parser.add_argument("--isMapped", default = True, type = bool, help = "") + parser.add_argument("--channel", default = "can0", help = "Which can channel to send messages on.") + parser.add_argument("--bustype", default = "socketcan", help = "The bus type") + parsed = parser.parse_args(args) + + can_bus = can.interface.Bus(channel=parsed.channel, bustype=parsed.bustype) + signal.signal(signal.SIGINT, getSignal(can_bus)) + + min_board = min(can_better_map.keys()) + max_board = max(can_better_map.keys()) + if(parsed.isMapped): + while True: + for thrusterNum in range(1, N_THRUSTERS + 1): + can_pow = [ZERO_THROTTLE] * N_THRUSTERS + can_pow[thrusterNum - 1] = LIL_FORWARD + + for cid in range(min_board, max_board + 1): + data_list = 0x7F_7F_7F_7F_00 + board = can_better_map[cid] + + if(thrusterNum not in board): + continue + + for thruster in board: + if thruster: + data_list += can_pow[thruster - 1] + else: + data_list += ZERO_THROTTLE + + data_list = data_list << 8 + data_list = data_list >> 8 + data_list_send = list() + shift = 64 + for i in range(0, 8): + shift -= 8 + data_list_send.append((data_list >> shift) % 256) + data = bytearray(data_list_send) + + print(f"Thruster {thrusterNum} to board 0x{cid:x} writing {data_list_send}") + writeToCan({cid: data}) + time.sleep(2) + writeToCan({cid: HALT_BYTE_ARRAY}) + time.sleep(2) + + else: + while True: + for can_id in range(min_board, max_board + 1): + for motor_index in range(1, 4 + 1): + packet = deepcopy(BASE_PACKET) + packet[can_id][3 + motor_index] = LIL_FORWARD + writeToCan(packet) + print(f"Firing ESC 0x{can_id:x} motor {motor_index}") + time.sleep(DELAY) + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/ros/src/utils/src/motor_drive_intf.py b/ros/src/utils/src/motor_drive_intf.py new file mode 100644 index 0000000..9717a2c --- /dev/null +++ b/ros/src/utils/src/motor_drive_intf.py @@ -0,0 +1,76 @@ +#! /usr/bin/python3 +""" +This file is for running a motor and being able to set speeds for +a motor or thruster. It is intended as a backup for the ESC Black +Box or to test with the full ROV. +""" + + +import argparse +from copy import deepcopy +import signal +import sys + +import can + +from thrust_range_test import writeToCan, getSignal, zeroOutThrusters, N_CAN_BYTES, ZERO_THROTTLE +from esc_1by1_test import BASE_PACKET + + +def discoverCanId() -> int: + """""" + canIdStr = input("Enter the ESC CAN ID (usually 0x201-0x203) to be driven: ").strip().lower() + base = 10 + 6 * ('x' in canIdStr) + try: + canId = int(canIdStr, base = base) + except ValueError: + print(f"Error: Not a valid decimal or hexadecimal ID entered. Got {canIdStr}.") + sys.exit(1) + return canId + + +def discoverEscId() -> int: + """""" + escIdStr = input("Enter which ESC to be driven (1-4): ").strip().lower() + try: + escId = int(escIdStr) + if(not 1 <= escId <= 4): + print(f"Error: Need an esc number between 1 and 4, not {escId}.") + sys.exit(1) + except ValueError: + print(f"Error: Not a valid number, got {escIdStr}") + sys.exit(1) + return escId + + +def main(args: list) -> None: + """""" + parser = argparse.ArgumentParser(description = __doc__) + parser.add_argument("--isMapped", default = True, type = bool, help = "") + parser.add_argument("--channel", default = "can0", help = "Which can channel to send messages on.") + parser.add_argument("--bustype", default = "socketcan", help = "The bus type") + parsed = parser.parse_args(args) + + can_bus = can.interface.Bus(channel = parsed.channel, bustype = parsed.bustype) + + signal.signal(signal.SIGINT, getSignal(can_bus)) + + canId = discoverCanId() + escNum = discoverEscId() + + print(f"Enter speeds for the ESC. 0 is full reverse, {ZERO_THROTTLE} is stationary, 255 is full forward.") + while(True): + try: + speed = int(input("Speed>").strip()) + if(speed > 255 or speed < 0): + raise ValueError() + packet = deepcopy(BASE_PACKET) + packet[canId][4 + escNum] = speed + writeToCan(packet) + except ValueError: + zeroOutThrusters() + sys.exit(1) + + +if(__name__ == "__main__"): + main(sys.argv[1:]) diff --git a/ros/src/utils/src/solenoid_test.py b/ros/src/utils/src/solenoid_test.py new file mode 100755 index 0000000..0452078 --- /dev/null +++ b/ros/src/utils/src/solenoid_test.py @@ -0,0 +1,67 @@ +#! /usr/bin/python3 +"""This script is good for testing the individual actuation and mapping of bits to +solenoids and tools. It can be run either statefully or non-statefully. Stateful +will keep previous solenoids on before "circling back" to turn them off one by one. +""" + +import argparse +import sys +import time + +import can + +from thrust_range_test import N_CAN_BYTES + +DELAY = 1.5 + + +def main(args: list) -> None: + parser = argparse.ArgumentParser(description = __doc__) + parser.add_argument("--channel", default = "can0", help = "The CAN channel to use") + parser.add_argument("--wait", default = DELAY, type = int, help = "The time to wait between toggling solenoids.") + parser.add_argument("--canId", default = 0x204, type = int, help = "The CAN ID of the solenoid board.") + parser.add_argument("--byteIndex", default = None, type = int, help = "The index of the solenoid control byte in the 8 CAN packet bytes.") + parser.add_argument("--stateful", default = False, type = bool, help = "Whether to turn solenoids on one by one (False) or turn them all on one by one then off one by one (True).") + parsed = parser.parse_args(args) + + channel = parsed.channel + can_bus = can.interface.Bus(channel = channel, bustype = 'socketcan') + # If you are not sure which byte of the 8 that are sent in a CAN packet is used + # by the solenoid board, then set this value to none. Otherwise, set it between 0 and 7. + solenoidByteIndex = parsed.byteIndex + # Setting stateful to true will go through and toggle each solenoid such that they + # won't turn off until the loop comes around a second time. + stateful = parsed.stateful + can_id = parsed.canId + + if solenoidByteIndex is not None and not (0 <= solenoidByteIndex <= 7): + raise ValueError(f"solenoidByteIndex must be None or between 0 and 7, not {solenoidByteIndex}.") + + print(f"Running {'' if stateful else 'non-'}statefully on {'all indices' if solenoidByteIndex is None else f'index {solenoidByteIndex}'}") + + lastValue = 0x0 + while True: + for bit_index in reversed(range(7+1)): + value = 1 << bit_index + if stateful: + value = lastValue ^ value + lastValue = value + + if solenoidByteIndex is None: + data_array = [value] * N_CAN_BYTES + else: + data_array = [0] * N_CAN_BYTES + data_array[solenoidByteIndex] = value + + data = bytearray(data_array) + + print(f"SOL: {value:#010b} hex: 0x{value:02X} index: {bit_index}") + can_tx = can.Message(arbitration_id=can_id, data=data, extended_id=False) + can_bus.send(can_tx, timeout=0.1) + + time.sleep(DELAY) + print('') + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/ros/src/utils/src/solenoid_test_all.py b/ros/src/utils/src/solenoid_test_all.py new file mode 100755 index 0000000..3dae23c --- /dev/null +++ b/ros/src/utils/src/solenoid_test_all.py @@ -0,0 +1,42 @@ +#! /usr/bin/python3 +"""This script alternates with all the solenoids on and all the solenoids off. +Useful for checking if anything works or flushing lines after a pool test. +""" + +import argparse +import sys +import time + +import can + +from thrust_range_test import N_CAN_BYTES + +DELAY = 1.5 + + +def main(args: list) -> None: + """""" + parser = argparse.ArgumentParser(description = __doc__) + parser.add_argument("--channel", default = "can0", help = "The CAN channel to use") + parser.add_argument("--wait", default = DELAY, type = int, help = "The time to wait between toggling solenoids.") + parser.add_argument("--canId", default = 0x204, type = int, help = "The CAN ID of the solenoid board.") + parsed = parser.parse_args(args) + channel = parsed.channel + can_bus = can.interface.Bus(channel=channel, bustype='socketcan') + can_id = parsed.canId + + values = [0xFF, 0x00] + + while True: + for v in values: + data = bytearray([v] * N_CAN_BYTES) + + print(f"SOL: {v:#010b} hex: 0x{v:02X}") + can_tx = can.Message(arbitration_id=can_id, data=data, extended_id=False) + can_bus.send(can_tx, timeout=0.1) + + time.sleep(DELAY) + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/ros/src/can_com/scripts/thrust_brown_test_grant.py b/ros/src/utils/src/thrust_brown_test_grant.py similarity index 100% rename from ros/src/can_com/scripts/thrust_brown_test_grant.py rename to ros/src/utils/src/thrust_brown_test_grant.py diff --git a/ros/src/can_com/scripts/thrust_range_test.py b/ros/src/utils/src/thrust_range_test.py similarity index 76% rename from ros/src/can_com/scripts/thrust_range_test.py rename to ros/src/utils/src/thrust_range_test.py index 280b932..4dcb1a0 100644 --- a/ros/src/can_com/scripts/thrust_range_test.py +++ b/ros/src/utils/src/thrust_range_test.py @@ -1,13 +1,17 @@ #! /usr/bin/python +import signal import sys -import can import time -import signal + +import can + +N_CAN_BYTES = 8 +ZERO_THROTTLE = 127 def getSignal(bus): - def signal_handler(sig, frame): + def signal_handler(sig, frame) -> None: print("CTRL+C detected") zeroOutThrusters(bus=bus) print("Thrusters zero-ed out") @@ -16,18 +20,17 @@ def signal_handler(sig, frame): return signal_handler -def zeroOutThrusters(bus=None): - a = mapThrusters([127] * 8) +def zeroOutThrusters(bus=None) -> None: + a = mapThrusters([ZERO_THROTTLE] * N_CAN_BYTES) writeToCan(a, timesleep=0.0, bus=bus, printOut=True) -def mapThrusters(can_pow, can_map=None, printOut=False): +def mapThrusters(can_pow, can_map=None, printOut=False) -> dict: if can_map is None: can_map = { 0x201: [7, 0, 0, 0], - 0x202: [0, 4, 5, 6], - 0x203: [0, 1, 2, 3] + 0x202: [0, 4, 5, 6] } can_out = {} @@ -53,22 +56,21 @@ def mapThrusters(can_pow, can_map=None, printOut=False): return can_out -def writeToCan(packet, timesleep=1, bus=None, printOut=False): +def writeToCan(packet, timesleep=1, bus=None, printOut=False) -> None: if bus is None: bus = can.interface.Bus(channel='can0', bustype='socketcan') for cid in packet: - # print(packet[cid]) data = bytearray(packet[cid]) - can_tx = can.Message(arbitration_id=cid, data=data, extended_id=False) + can_tx = can.Message(arbitration_id=cid, data=data,is_extended_id=False) bus.send(can_tx, timeout=1) if printOut: - tst = " {}:".format(cid) + tst = f" 0x{cid:x}:" for el in data: - tst += " {0:03}".format(int(el)) + tst += f" {int(el):03}" print(tst) @@ -104,6 +106,5 @@ def mainLoop(timesleep=1, bound=5, increment=1, mid=127, channel='can0', bustype if __name__ == "__main__": bound = 10 * 10 - #bound = 50 inc = 2 - print(mainLoop(bound=bound, increment=inc, timesleep=.1)) + print(mainLoop(bound=bound, increment=inc, timesleep=0.1)) diff --git a/surface/.gitignore b/surface/.gitignore index 19193e4..6234fd8 100644 --- a/surface/.gitignore +++ b/surface/.gitignore @@ -3,4 +3,5 @@ dist/*.js* package-lock.json **/__pycache__ **/build -**/devel \ No newline at end of file +**/devel +**/cv/testing \ No newline at end of file diff --git a/surface/compros.sh b/surface/compros.sh new file mode 100644 index 0000000..2152c18 --- /dev/null +++ b/surface/compros.sh @@ -0,0 +1,6 @@ +#! /bin/bash + +export ROS_IP=$(ifconfig | grep -Eo 'inet (addr:)?([0-9]*\.){3}[0-9]*' | grep -Eo '([0-9]*\.){3}[0-9]*' | grep 192) +export ROS_MASTER_URI=http://192.168.1.3:11311 +export ROS_HOSTNAME=$(ifconfig | grep -Eo 'inet (addr:)?([0-9]*\.){3}[0-9]*' | grep -Eo '([0-9]*\.){3}[0-9]*' | grep 192) + diff --git a/surface/cv/process.py b/surface/cv/process.py new file mode 100644 index 0000000..dc3b26b --- /dev/null +++ b/surface/cv/process.py @@ -0,0 +1,197 @@ +import cv2 +from matplotlib import pyplot as plt +import os +import numpy as np + + +def color_diff(color0, color1): + return abs(int(color0) - int(color1)) + + +def color_down(image, thresh): + row = int(image.shape[0] / 2) #start at center, move towards edge looking for colors + col = int(image.shape[1] / 2) + new_row = row + 1 + new_col = col #+ 1 + baseline = image[row][col] + + while(new_row < image.shape[0]): + if( color_diff(baseline, image[new_row][new_col]) > thresh ): + break + new_row += 1 + + if new_row == image.shape[0]: + return new_row - 1 + return new_row + + +def color_left(image, thresh): + row = int(image.shape[0] / 2) + col = int(image.shape[1] / 2) + new_row = row #- 1 + new_col = col - 1 + baseline = image[row][col] + + while(new_col >= 0): + if( color_diff(baseline, image[new_row][new_col]) > thresh ): + break + new_col -= 1 + + if new_col < 0: + return 0 + return new_col + + +def color_up(image, thresh): + row = int(image.shape[0] / 2) + col = int(image.shape[1] / 2) + new_row = row - 1 + new_col = col #+ 1 + baseline = image[row][col] + + while(new_row >= 0): + if( color_diff(baseline, image[new_row][new_col]) > thresh ): + break + new_row -= 1 + + if new_row < 0: + return 0 + return new_row + + +def color_right(image, thresh): + row = int(image.shape[0] / 2) + col = int(image.shape[1] / 2) + new_row = row #+ 1 + new_col = col + 1 + baseline = image[row][col] + + while(new_col < image.shape[1]): + if( color_diff(baseline, image[new_row][new_col]) > thresh ): + break + new_col += 1 + + if new_col == image.shape[1]: + return new_col - 1 + return new_col + + +def crops_and_colors(brg, image, thresh, crop_delta, color_delta): + down = color_down(image, thresh) + left = color_left(image, thresh) + up = color_up(image, thresh) + right = color_right(image, thresh) + + new_image = image.copy() #so I can draw on the picture in testing + + #grab better color pixel + downc = image.shape[0]-1 if down + color_delta >= image.shape[0] else down + color_delta + leftc = 0 if left - color_delta < 0 else left - color_delta + upc = 0 if up - color_delta < 0 else up - color_delta + rightc = image.shape[1]-1 if right + color_delta >= image.shape[1] else right + color_delta + + #color extracting + row = int(new_image.shape[0] / 2) + col = int(new_image.shape[1] / 2) + + image_struct = { 'down': image[downc][col], + 'left': image[row][leftc], + 'up': image[upc][col], + 'right': image[row][rightc] } + + #expand beyond color start for cropping purposes + downd = image.shape[0] if down + crop_delta > image.shape[0] else down + crop_delta + leftd = 0 if left - crop_delta < 0 else left - crop_delta + upd = 0 if up - crop_delta < 0 else up - crop_delta + rightd = image.shape[1] if right + crop_delta > image.shape[1] else right + crop_delta + + newer_image = brg[upd:downd, leftd:rightd, :] #y:y+h, x:x+w, z + + return image_struct, newer_image + + +def compile_mosaic(mosaic): + #push everything down and left + tallest = max( + mosaic['left_square']['image'].shape[0], + mosaic['rect_bot']['image'].shape[0], + mosaic['right_square']['image'].shape[0], + mosaic['rect_right']['image'].shape[0] + ) + widest = mosaic['top']['image'].shape[1] if mosaic['top']['image'].shape[1] > mosaic['rect_bot']['image'].shape[1] else mosaic['rect_bot']['image'].shape[1] + + image_top = cv2.copyMakeBorder(mosaic['top']['image'], + 0, + tallest, + mosaic['left_square']['image'].shape[1], + widest-mosaic['top']['image'].shape[1] + mosaic['right_square']['image'].shape[1] + mosaic['rect_right']['image'].shape[1], + 0) + image_rect_bot = cv2.copyMakeBorder(mosaic['rect_bot']['image'], + tallest-mosaic['rect_bot']['image'].shape[0] + mosaic['top']['image'].shape[0], + 0, + mosaic['left_square']['image'].shape[1], + widest-mosaic['rect_bot']['image'].shape[1] + mosaic['right_square']['image'].shape[1] + mosaic['rect_right']['image'].shape[1], + 0) + image_right_square = cv2.copyMakeBorder(mosaic['right_square']['image'], + tallest-mosaic['right_square']['image'].shape[0] + mosaic['top']['image'].shape[0], + 0, + mosaic['left_square']['image'].shape[1] + widest, + mosaic['rect_right']['image'].shape[1], + 0) + image_rect_right = cv2.copyMakeBorder(mosaic['rect_right']['image'], + tallest-mosaic['rect_right']['image'].shape[0] + mosaic['top']['image'].shape[0], + 0, + mosaic['left_square']['image'].shape[1] + widest + mosaic['right_square']['image'].shape[1], + 0, + 0) + image_left_square = cv2.copyMakeBorder(mosaic['left_square']['image'], + tallest-mosaic['left_square']['image'].shape[0] + mosaic['top']['image'].shape[0], + 0, + 0, + widest + mosaic['right_square']['image'].shape[1] + mosaic['rect_right']['image'].shape[1], + 0) + + shape = (tallest + mosaic['top']['image'].shape[0], + mosaic['left_square']['image'].shape[1] + widest + mosaic['right_square']['image'].shape[1] + mosaic['rect_right']['image'].shape[1], + 3) + final = np.zeros(shape, np.uint8) + return final + image_top + image_rect_bot + image_right_square + image_rect_right + image_left_square + + +def image_folder_read(path): + images = [ cv2.imread(path+j) for j in sorted( [i for i in os.listdir(path)] ) ] + if not(len(images) >= 5): + print('Incorrect number of images, there should be exactly 5') + exit() + return images + + +if __name__ == "__main__": + path = './testing/' + thresh = 10 #difference from baseline saturation + crop_delta = 100 #color overflow to edge + color_delta = 25 + + images = image_folder_read(path) + + mosaic = {} + + for i, image in enumerate(images): + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + sat = hsv[:,:,1] + + image_struct, new_image = crops_and_colors(image, sat, thresh, crop_delta, color_delta) + + if i == 0: + mosaic['top'] = {'image_struct': image_struct, 'image': new_image} + if i == 1: + mosaic['rect_bot'] = {'image_struct': image_struct, 'image': new_image,} + if i == 2: + mosaic['right_square'] = {'image_struct': image_struct, 'image': new_image,} + if i == 3: + mosaic['rect_right'] = {'image_struct': image_struct, 'image': new_image,} + if i == 4: + mosaic['left_square'] = {'image_struct': image_struct, 'image': new_image} + + final = compile_mosaic(mosaic) + cv2.imwrite('./testing/done.png', final) \ No newline at end of file diff --git a/surface/cv/stream.py b/surface/cv/stream.py new file mode 100644 index 0000000..53bfb01 --- /dev/null +++ b/surface/cv/stream.py @@ -0,0 +1,80 @@ +import cv2 +import socket +import threading +import time +import sys +import signal +import os + +capture = False +counter = 0 + +class SocketManager: + def __init__(self, port): + self.running = True + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sock.bind(('127.0.0.1', port)) + self.sock.listen(5) + self.sock.settimeout(1) + self.connected = False + + self.thread = threading.Thread(target=self.run) + self.thread.start() + + def shutdown(self): + self.running = False + self.sock.close() + self.thread.join() + + def run(self): + global capture, counter + while not self.connected and self.running: + try: + conn, addr = self.sock.accept() + self.connected = True + except: + pass + time.sleep(1) + while self.running: + try: + data = conn.recv(1024) + except: + pass + if data: + capture = len(data.decode()) > 0 + counter = int(data.decode()) + print(capture) + print(counter) + +def shutdown(sig, frame): + global input_socket + print('shutting down') + input_socket.shutdown() + +if __name__ == '__main__': + global input_socket + + signal.signal(signal.SIGINT, shutdown) + signal.signal(signal.SIGTERM, shutdown) + + input_socket = SocketManager(int(sys.argv[1])) + + vcap = cv2.VideoCapture('http://192.168.1.3:8090/cam0') + + print('ready') + + while(input_socket.running): + try: + ret, frame = vcap.read() + except: + continue + + if capture and ret: + #Save image + print(os.path.curdir) + cv2.imwrite(f'cv/testing/{counter}.png', frame) + capture = False + + time.sleep(0.5) + + input_socket.shutdown() \ No newline at end of file diff --git a/surface/dist/electron.js b/surface/dist/electron.js deleted file mode 100644 index ec2d750..0000000 --- a/surface/dist/electron.js +++ /dev/null @@ -1,473 +0,0 @@ -/* - * ATTENTION: The "eval" devtool has been used (maybe by default in mode: "development"). - * This devtool is neither made for production nor for readable output files. - * It uses "eval()" calls to create a separate source file in the browser devtools. - * If you are trying to read the output file, select a different devtool (https://webpack.js.org/configuration/devtool/) - * or disable the default devtool with "devtool: false". - * If you are looking for production-ready output files, see mode: "production" (https://webpack.js.org/configuration/mode/). - */ -/******/ (() => { // webpackBootstrap -/******/ var __webpack_modules__ = ({ - -/***/ "./node_modules/buffer-from/index.js": -/*!*******************************************!*\ - !*** ./node_modules/buffer-from/index.js ***! - \*******************************************/ -/***/ ((module) => { - -eval("var toString = Object.prototype.toString\n\nvar isModern = (\n typeof Buffer.alloc === 'function' &&\n typeof Buffer.allocUnsafe === 'function' &&\n typeof Buffer.from === 'function'\n)\n\nfunction isArrayBuffer (input) {\n return toString.call(input).slice(8, -1) === 'ArrayBuffer'\n}\n\nfunction fromArrayBuffer (obj, byteOffset, length) {\n byteOffset >>>= 0\n\n var maxLength = obj.byteLength - byteOffset\n\n if (maxLength < 0) {\n throw new RangeError(\"'offset' is out of bounds\")\n }\n\n if (length === undefined) {\n length = maxLength\n } else {\n length >>>= 0\n\n if (length > maxLength) {\n throw new RangeError(\"'length' is out of bounds\")\n }\n }\n\n return isModern\n ? Buffer.from(obj.slice(byteOffset, byteOffset + length))\n : new Buffer(new Uint8Array(obj.slice(byteOffset, byteOffset + length)))\n}\n\nfunction fromString (string, encoding) {\n if (typeof encoding !== 'string' || encoding === '') {\n encoding = 'utf8'\n }\n\n if (!Buffer.isEncoding(encoding)) {\n throw new TypeError('\"encoding\" must be a valid string encoding')\n }\n\n return isModern\n ? Buffer.from(string, encoding)\n : new Buffer(string, encoding)\n}\n\nfunction bufferFrom (value, encodingOrOffset, length) {\n if (typeof value === 'number') {\n throw new TypeError('\"value\" argument must not be a number')\n }\n\n if (isArrayBuffer(value)) {\n return fromArrayBuffer(value, encodingOrOffset, length)\n }\n\n if (typeof value === 'string') {\n return fromString(value, encodingOrOffset)\n }\n\n return isModern\n ? Buffer.from(value)\n : new Buffer(value)\n}\n\nmodule.exports = bufferFrom\n\n\n//# sourceURL=webpack://electron-test/./node_modules/buffer-from/index.js?"); - -/***/ }), - -/***/ "./node_modules/concat-stream/index.js": -/*!*********************************************!*\ - !*** ./node_modules/concat-stream/index.js ***! - \*********************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -eval("var Writable = __webpack_require__(/*! readable-stream */ \"./node_modules/readable-stream/readable.js\").Writable\nvar inherits = __webpack_require__(/*! inherits */ \"./node_modules/inherits/inherits.js\")\nvar bufferFrom = __webpack_require__(/*! buffer-from */ \"./node_modules/buffer-from/index.js\")\n\nif (typeof Uint8Array === 'undefined') {\n var U8 = __webpack_require__(/*! typedarray */ \"./node_modules/typedarray/index.js\").Uint8Array\n} else {\n var U8 = Uint8Array\n}\n\nfunction ConcatStream(opts, cb) {\n if (!(this instanceof ConcatStream)) return new ConcatStream(opts, cb)\n\n if (typeof opts === 'function') {\n cb = opts\n opts = {}\n }\n if (!opts) opts = {}\n\n var encoding = opts.encoding\n var shouldInferEncoding = false\n\n if (!encoding) {\n shouldInferEncoding = true\n } else {\n encoding = String(encoding).toLowerCase()\n if (encoding === 'u8' || encoding === 'uint8') {\n encoding = 'uint8array'\n }\n }\n\n Writable.call(this, { objectMode: true })\n\n this.encoding = encoding\n this.shouldInferEncoding = shouldInferEncoding\n\n if (cb) this.on('finish', function () { cb(this.getBody()) })\n this.body = []\n}\n\nmodule.exports = ConcatStream\ninherits(ConcatStream, Writable)\n\nConcatStream.prototype._write = function(chunk, enc, next) {\n this.body.push(chunk)\n next()\n}\n\nConcatStream.prototype.inferEncoding = function (buff) {\n var firstBuffer = buff === undefined ? this.body[0] : buff;\n if (Buffer.isBuffer(firstBuffer)) return 'buffer'\n if (typeof Uint8Array !== 'undefined' && firstBuffer instanceof Uint8Array) return 'uint8array'\n if (Array.isArray(firstBuffer)) return 'array'\n if (typeof firstBuffer === 'string') return 'string'\n if (Object.prototype.toString.call(firstBuffer) === \"[object Object]\") return 'object'\n return 'buffer'\n}\n\nConcatStream.prototype.getBody = function () {\n if (!this.encoding && this.body.length === 0) return []\n if (this.shouldInferEncoding) this.encoding = this.inferEncoding()\n if (this.encoding === 'array') return arrayConcat(this.body)\n if (this.encoding === 'string') return stringConcat(this.body)\n if (this.encoding === 'buffer') return bufferConcat(this.body)\n if (this.encoding === 'uint8array') return u8Concat(this.body)\n return this.body\n}\n\nvar isArray = Array.isArray || function (arr) {\n return Object.prototype.toString.call(arr) == '[object Array]'\n}\n\nfunction isArrayish (arr) {\n return /Array\\]$/.test(Object.prototype.toString.call(arr))\n}\n\nfunction isBufferish (p) {\n return typeof p === 'string' || isArrayish(p) || (p && typeof p.subarray === 'function')\n}\n\nfunction stringConcat (parts) {\n var strings = []\n var needsToString = false\n for (var i = 0; i < parts.length; i++) {\n var p = parts[i]\n if (typeof p === 'string') {\n strings.push(p)\n } else if (Buffer.isBuffer(p)) {\n strings.push(p)\n } else if (isBufferish(p)) {\n strings.push(bufferFrom(p))\n } else {\n strings.push(bufferFrom(String(p)))\n }\n }\n if (Buffer.isBuffer(parts[0])) {\n strings = Buffer.concat(strings)\n strings = strings.toString('utf8')\n } else {\n strings = strings.join('')\n }\n return strings\n}\n\nfunction bufferConcat (parts) {\n var bufs = []\n for (var i = 0; i < parts.length; i++) {\n var p = parts[i]\n if (Buffer.isBuffer(p)) {\n bufs.push(p)\n } else if (isBufferish(p)) {\n bufs.push(bufferFrom(p))\n } else {\n bufs.push(bufferFrom(String(p)))\n }\n }\n return Buffer.concat(bufs)\n}\n\nfunction arrayConcat (parts) {\n var res = []\n for (var i = 0; i < parts.length; i++) {\n res.push.apply(res, parts[i])\n }\n return res\n}\n\nfunction u8Concat (parts) {\n var len = 0\n for (var i = 0; i < parts.length; i++) {\n if (typeof parts[i] === 'string') {\n parts[i] = bufferFrom(parts[i])\n }\n len += parts[i].length\n }\n var u8 = new U8(len)\n for (var i = 0, offset = 0; i < parts.length; i++) {\n var part = parts[i]\n for (var j = 0; j < part.length; j++) {\n u8[offset++] = part[j]\n }\n }\n return u8\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/concat-stream/index.js?"); - -/***/ }), - -/***/ "./node_modules/core-util-is/lib/util.js": -/*!***********************************************!*\ - !*** ./node_modules/core-util-is/lib/util.js ***! - \***********************************************/ -/***/ ((__unused_webpack_module, exports) => { - -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n// NOTE: These type checking functions intentionally don't use `instanceof`\n// because it is fragile and can be easily faked with `Object.create()`.\n\nfunction isArray(arg) {\n if (Array.isArray) {\n return Array.isArray(arg);\n }\n return objectToString(arg) === '[object Array]';\n}\nexports.isArray = isArray;\n\nfunction isBoolean(arg) {\n return typeof arg === 'boolean';\n}\nexports.isBoolean = isBoolean;\n\nfunction isNull(arg) {\n return arg === null;\n}\nexports.isNull = isNull;\n\nfunction isNullOrUndefined(arg) {\n return arg == null;\n}\nexports.isNullOrUndefined = isNullOrUndefined;\n\nfunction isNumber(arg) {\n return typeof arg === 'number';\n}\nexports.isNumber = isNumber;\n\nfunction isString(arg) {\n return typeof arg === 'string';\n}\nexports.isString = isString;\n\nfunction isSymbol(arg) {\n return typeof arg === 'symbol';\n}\nexports.isSymbol = isSymbol;\n\nfunction isUndefined(arg) {\n return arg === void 0;\n}\nexports.isUndefined = isUndefined;\n\nfunction isRegExp(re) {\n return objectToString(re) === '[object RegExp]';\n}\nexports.isRegExp = isRegExp;\n\nfunction isObject(arg) {\n return typeof arg === 'object' && arg !== null;\n}\nexports.isObject = isObject;\n\nfunction isDate(d) {\n return objectToString(d) === '[object Date]';\n}\nexports.isDate = isDate;\n\nfunction isError(e) {\n return (objectToString(e) === '[object Error]' || e instanceof Error);\n}\nexports.isError = isError;\n\nfunction isFunction(arg) {\n return typeof arg === 'function';\n}\nexports.isFunction = isFunction;\n\nfunction isPrimitive(arg) {\n return arg === null ||\n typeof arg === 'boolean' ||\n typeof arg === 'number' ||\n typeof arg === 'string' ||\n typeof arg === 'symbol' || // ES6 symbol\n typeof arg === 'undefined';\n}\nexports.isPrimitive = isPrimitive;\n\nexports.isBuffer = Buffer.isBuffer;\n\nfunction objectToString(o) {\n return Object.prototype.toString.call(o);\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/core-util-is/lib/util.js?"); - -/***/ }), - -/***/ "./node_modules/inherits/inherits.js": -/*!*******************************************!*\ - !*** ./node_modules/inherits/inherits.js ***! - \*******************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -eval("try {\n var util = __webpack_require__(/*! util */ \"util\");\n /* istanbul ignore next */\n if (typeof util.inherits !== 'function') throw '';\n module.exports = util.inherits;\n} catch (e) {\n /* istanbul ignore next */\n module.exports = __webpack_require__(/*! ./inherits_browser.js */ \"./node_modules/inherits/inherits_browser.js\");\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/inherits/inherits.js?"); - -/***/ }), - -/***/ "./node_modules/inherits/inherits_browser.js": -/*!***************************************************!*\ - !*** ./node_modules/inherits/inherits_browser.js ***! - \***************************************************/ -/***/ ((module) => { - -eval("if (typeof Object.create === 'function') {\n // implementation from standard node.js 'util' module\n module.exports = function inherits(ctor, superCtor) {\n if (superCtor) {\n ctor.super_ = superCtor\n ctor.prototype = Object.create(superCtor.prototype, {\n constructor: {\n value: ctor,\n enumerable: false,\n writable: true,\n configurable: true\n }\n })\n }\n };\n} else {\n // old school shim for old browsers\n module.exports = function inherits(ctor, superCtor) {\n if (superCtor) {\n ctor.super_ = superCtor\n var TempCtor = function () {}\n TempCtor.prototype = superCtor.prototype\n ctor.prototype = new TempCtor()\n ctor.prototype.constructor = ctor\n }\n }\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/inherits/inherits_browser.js?"); - -/***/ }), - -/***/ "./node_modules/isarray/index.js": -/*!***************************************!*\ - !*** ./node_modules/isarray/index.js ***! - \***************************************/ -/***/ ((module) => { - -eval("var toString = {}.toString;\n\nmodule.exports = Array.isArray || function (arr) {\n return toString.call(arr) == '[object Array]';\n};\n\n\n//# sourceURL=webpack://electron-test/./node_modules/isarray/index.js?"); - -/***/ }), - -/***/ "./node_modules/process-nextick-args/index.js": -/*!****************************************************!*\ - !*** ./node_modules/process-nextick-args/index.js ***! - \****************************************************/ -/***/ ((module) => { - -"use strict"; -eval("\n\nif (typeof process === 'undefined' ||\n !process.version ||\n process.version.indexOf('v0.') === 0 ||\n process.version.indexOf('v1.') === 0 && process.version.indexOf('v1.8.') !== 0) {\n module.exports = { nextTick: nextTick };\n} else {\n module.exports = process\n}\n\nfunction nextTick(fn, arg1, arg2, arg3) {\n if (typeof fn !== 'function') {\n throw new TypeError('\"callback\" argument must be a function');\n }\n var len = arguments.length;\n var args, i;\n switch (len) {\n case 0:\n case 1:\n return process.nextTick(fn);\n case 2:\n return process.nextTick(function afterTickOne() {\n fn.call(null, arg1);\n });\n case 3:\n return process.nextTick(function afterTickTwo() {\n fn.call(null, arg1, arg2);\n });\n case 4:\n return process.nextTick(function afterTickThree() {\n fn.call(null, arg1, arg2, arg3);\n });\n default:\n args = new Array(len - 1);\n i = 0;\n while (i < args.length) {\n args[i++] = arguments[i];\n }\n return process.nextTick(function afterTick() {\n fn.apply(null, args);\n });\n }\n}\n\n\n\n//# sourceURL=webpack://electron-test/./node_modules/process-nextick-args/index.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/_stream_duplex.js": -/*!************************************************************!*\ - !*** ./node_modules/readable-stream/lib/_stream_duplex.js ***! - \************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n// a duplex stream is just a stream that is both readable and writable.\n// Since JS doesn't have multiple prototypal inheritance, this class\n// prototypally inherits from Readable, and then parasitically from\n// Writable.\n\n\n\n/**/\n\nvar pna = __webpack_require__(/*! process-nextick-args */ \"./node_modules/process-nextick-args/index.js\");\n/**/\n\n/**/\nvar objectKeys = Object.keys || function (obj) {\n var keys = [];\n for (var key in obj) {\n keys.push(key);\n }return keys;\n};\n/**/\n\nmodule.exports = Duplex;\n\n/**/\nvar util = Object.create(__webpack_require__(/*! core-util-is */ \"./node_modules/core-util-is/lib/util.js\"));\nutil.inherits = __webpack_require__(/*! inherits */ \"./node_modules/inherits/inherits.js\");\n/**/\n\nvar Readable = __webpack_require__(/*! ./_stream_readable */ \"./node_modules/readable-stream/lib/_stream_readable.js\");\nvar Writable = __webpack_require__(/*! ./_stream_writable */ \"./node_modules/readable-stream/lib/_stream_writable.js\");\n\nutil.inherits(Duplex, Readable);\n\n{\n // avoid scope creep, the keys array can then be collected\n var keys = objectKeys(Writable.prototype);\n for (var v = 0; v < keys.length; v++) {\n var method = keys[v];\n if (!Duplex.prototype[method]) Duplex.prototype[method] = Writable.prototype[method];\n }\n}\n\nfunction Duplex(options) {\n if (!(this instanceof Duplex)) return new Duplex(options);\n\n Readable.call(this, options);\n Writable.call(this, options);\n\n if (options && options.readable === false) this.readable = false;\n\n if (options && options.writable === false) this.writable = false;\n\n this.allowHalfOpen = true;\n if (options && options.allowHalfOpen === false) this.allowHalfOpen = false;\n\n this.once('end', onend);\n}\n\nObject.defineProperty(Duplex.prototype, 'writableHighWaterMark', {\n // making it explicit this property is not enumerable\n // because otherwise some prototype manipulation in\n // userland will fail\n enumerable: false,\n get: function () {\n return this._writableState.highWaterMark;\n }\n});\n\n// the no-half-open enforcer\nfunction onend() {\n // if we allow half-open state, or if the writable side ended,\n // then we're ok.\n if (this.allowHalfOpen || this._writableState.ended) return;\n\n // no more data can be written.\n // But allow more writes to happen in this tick.\n pna.nextTick(onEndNT, this);\n}\n\nfunction onEndNT(self) {\n self.end();\n}\n\nObject.defineProperty(Duplex.prototype, 'destroyed', {\n get: function () {\n if (this._readableState === undefined || this._writableState === undefined) {\n return false;\n }\n return this._readableState.destroyed && this._writableState.destroyed;\n },\n set: function (value) {\n // we ignore the value if the stream\n // has not been initialized yet\n if (this._readableState === undefined || this._writableState === undefined) {\n return;\n }\n\n // backward compatibility, the user is explicitly\n // managing destroyed\n this._readableState.destroyed = value;\n this._writableState.destroyed = value;\n }\n});\n\nDuplex.prototype._destroy = function (err, cb) {\n this.push(null);\n this.end();\n\n pna.nextTick(cb, err);\n};\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/_stream_duplex.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/_stream_passthrough.js": -/*!*****************************************************************!*\ - !*** ./node_modules/readable-stream/lib/_stream_passthrough.js ***! - \*****************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n// a passthrough stream.\n// basically just the most minimal sort of Transform stream.\n// Every written chunk gets output as-is.\n\n\n\nmodule.exports = PassThrough;\n\nvar Transform = __webpack_require__(/*! ./_stream_transform */ \"./node_modules/readable-stream/lib/_stream_transform.js\");\n\n/**/\nvar util = Object.create(__webpack_require__(/*! core-util-is */ \"./node_modules/core-util-is/lib/util.js\"));\nutil.inherits = __webpack_require__(/*! inherits */ \"./node_modules/inherits/inherits.js\");\n/**/\n\nutil.inherits(PassThrough, Transform);\n\nfunction PassThrough(options) {\n if (!(this instanceof PassThrough)) return new PassThrough(options);\n\n Transform.call(this, options);\n}\n\nPassThrough.prototype._transform = function (chunk, encoding, cb) {\n cb(null, chunk);\n};\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/_stream_passthrough.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/_stream_readable.js": -/*!**************************************************************!*\ - !*** ./node_modules/readable-stream/lib/_stream_readable.js ***! - \**************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n\n\n/**/\n\nvar pna = __webpack_require__(/*! process-nextick-args */ \"./node_modules/process-nextick-args/index.js\");\n/**/\n\nmodule.exports = Readable;\n\n/**/\nvar isArray = __webpack_require__(/*! isarray */ \"./node_modules/isarray/index.js\");\n/**/\n\n/**/\nvar Duplex;\n/**/\n\nReadable.ReadableState = ReadableState;\n\n/**/\nvar EE = __webpack_require__(/*! events */ \"events\").EventEmitter;\n\nvar EElistenerCount = function (emitter, type) {\n return emitter.listeners(type).length;\n};\n/**/\n\n/**/\nvar Stream = __webpack_require__(/*! ./internal/streams/stream */ \"./node_modules/readable-stream/lib/internal/streams/stream.js\");\n/**/\n\n/**/\n\nvar Buffer = __webpack_require__(/*! safe-buffer */ \"./node_modules/safe-buffer/index.js\").Buffer;\nvar OurUint8Array = global.Uint8Array || function () {};\nfunction _uint8ArrayToBuffer(chunk) {\n return Buffer.from(chunk);\n}\nfunction _isUint8Array(obj) {\n return Buffer.isBuffer(obj) || obj instanceof OurUint8Array;\n}\n\n/**/\n\n/**/\nvar util = Object.create(__webpack_require__(/*! core-util-is */ \"./node_modules/core-util-is/lib/util.js\"));\nutil.inherits = __webpack_require__(/*! inherits */ \"./node_modules/inherits/inherits.js\");\n/**/\n\n/**/\nvar debugUtil = __webpack_require__(/*! util */ \"util\");\nvar debug = void 0;\nif (debugUtil && debugUtil.debuglog) {\n debug = debugUtil.debuglog('stream');\n} else {\n debug = function () {};\n}\n/**/\n\nvar BufferList = __webpack_require__(/*! ./internal/streams/BufferList */ \"./node_modules/readable-stream/lib/internal/streams/BufferList.js\");\nvar destroyImpl = __webpack_require__(/*! ./internal/streams/destroy */ \"./node_modules/readable-stream/lib/internal/streams/destroy.js\");\nvar StringDecoder;\n\nutil.inherits(Readable, Stream);\n\nvar kProxyEvents = ['error', 'close', 'destroy', 'pause', 'resume'];\n\nfunction prependListener(emitter, event, fn) {\n // Sadly this is not cacheable as some libraries bundle their own\n // event emitter implementation with them.\n if (typeof emitter.prependListener === 'function') return emitter.prependListener(event, fn);\n\n // This is a hack to make sure that our error handler is attached before any\n // userland ones. NEVER DO THIS. This is here only because this code needs\n // to continue to work with older versions of Node.js that do not include\n // the prependListener() method. The goal is to eventually remove this hack.\n if (!emitter._events || !emitter._events[event]) emitter.on(event, fn);else if (isArray(emitter._events[event])) emitter._events[event].unshift(fn);else emitter._events[event] = [fn, emitter._events[event]];\n}\n\nfunction ReadableState(options, stream) {\n Duplex = Duplex || __webpack_require__(/*! ./_stream_duplex */ \"./node_modules/readable-stream/lib/_stream_duplex.js\");\n\n options = options || {};\n\n // Duplex streams are both readable and writable, but share\n // the same options object.\n // However, some cases require setting options to different\n // values for the readable and the writable sides of the duplex stream.\n // These options can be provided separately as readableXXX and writableXXX.\n var isDuplex = stream instanceof Duplex;\n\n // object stream flag. Used to make read(n) ignore n and to\n // make all the buffer merging and length checks go away\n this.objectMode = !!options.objectMode;\n\n if (isDuplex) this.objectMode = this.objectMode || !!options.readableObjectMode;\n\n // the point at which it stops calling _read() to fill the buffer\n // Note: 0 is a valid value, means \"don't call _read preemptively ever\"\n var hwm = options.highWaterMark;\n var readableHwm = options.readableHighWaterMark;\n var defaultHwm = this.objectMode ? 16 : 16 * 1024;\n\n if (hwm || hwm === 0) this.highWaterMark = hwm;else if (isDuplex && (readableHwm || readableHwm === 0)) this.highWaterMark = readableHwm;else this.highWaterMark = defaultHwm;\n\n // cast to ints.\n this.highWaterMark = Math.floor(this.highWaterMark);\n\n // A linked list is used to store data chunks instead of an array because the\n // linked list can remove elements from the beginning faster than\n // array.shift()\n this.buffer = new BufferList();\n this.length = 0;\n this.pipes = null;\n this.pipesCount = 0;\n this.flowing = null;\n this.ended = false;\n this.endEmitted = false;\n this.reading = false;\n\n // a flag to be able to tell if the event 'readable'/'data' is emitted\n // immediately, or on a later tick. We set this to true at first, because\n // any actions that shouldn't happen until \"later\" should generally also\n // not happen before the first read call.\n this.sync = true;\n\n // whenever we return null, then we set a flag to say\n // that we're awaiting a 'readable' event emission.\n this.needReadable = false;\n this.emittedReadable = false;\n this.readableListening = false;\n this.resumeScheduled = false;\n\n // has it been destroyed\n this.destroyed = false;\n\n // Crypto is kind of old and crusty. Historically, its default string\n // encoding is 'binary' so we have to make this configurable.\n // Everything else in the universe uses 'utf8', though.\n this.defaultEncoding = options.defaultEncoding || 'utf8';\n\n // the number of writers that are awaiting a drain event in .pipe()s\n this.awaitDrain = 0;\n\n // if true, a maybeReadMore has been scheduled\n this.readingMore = false;\n\n this.decoder = null;\n this.encoding = null;\n if (options.encoding) {\n if (!StringDecoder) StringDecoder = __webpack_require__(/*! string_decoder/ */ \"./node_modules/string_decoder/lib/string_decoder.js\").StringDecoder;\n this.decoder = new StringDecoder(options.encoding);\n this.encoding = options.encoding;\n }\n}\n\nfunction Readable(options) {\n Duplex = Duplex || __webpack_require__(/*! ./_stream_duplex */ \"./node_modules/readable-stream/lib/_stream_duplex.js\");\n\n if (!(this instanceof Readable)) return new Readable(options);\n\n this._readableState = new ReadableState(options, this);\n\n // legacy\n this.readable = true;\n\n if (options) {\n if (typeof options.read === 'function') this._read = options.read;\n\n if (typeof options.destroy === 'function') this._destroy = options.destroy;\n }\n\n Stream.call(this);\n}\n\nObject.defineProperty(Readable.prototype, 'destroyed', {\n get: function () {\n if (this._readableState === undefined) {\n return false;\n }\n return this._readableState.destroyed;\n },\n set: function (value) {\n // we ignore the value if the stream\n // has not been initialized yet\n if (!this._readableState) {\n return;\n }\n\n // backward compatibility, the user is explicitly\n // managing destroyed\n this._readableState.destroyed = value;\n }\n});\n\nReadable.prototype.destroy = destroyImpl.destroy;\nReadable.prototype._undestroy = destroyImpl.undestroy;\nReadable.prototype._destroy = function (err, cb) {\n this.push(null);\n cb(err);\n};\n\n// Manually shove something into the read() buffer.\n// This returns true if the highWaterMark has not been hit yet,\n// similar to how Writable.write() returns true if you should\n// write() some more.\nReadable.prototype.push = function (chunk, encoding) {\n var state = this._readableState;\n var skipChunkCheck;\n\n if (!state.objectMode) {\n if (typeof chunk === 'string') {\n encoding = encoding || state.defaultEncoding;\n if (encoding !== state.encoding) {\n chunk = Buffer.from(chunk, encoding);\n encoding = '';\n }\n skipChunkCheck = true;\n }\n } else {\n skipChunkCheck = true;\n }\n\n return readableAddChunk(this, chunk, encoding, false, skipChunkCheck);\n};\n\n// Unshift should *always* be something directly out of read()\nReadable.prototype.unshift = function (chunk) {\n return readableAddChunk(this, chunk, null, true, false);\n};\n\nfunction readableAddChunk(stream, chunk, encoding, addToFront, skipChunkCheck) {\n var state = stream._readableState;\n if (chunk === null) {\n state.reading = false;\n onEofChunk(stream, state);\n } else {\n var er;\n if (!skipChunkCheck) er = chunkInvalid(state, chunk);\n if (er) {\n stream.emit('error', er);\n } else if (state.objectMode || chunk && chunk.length > 0) {\n if (typeof chunk !== 'string' && !state.objectMode && Object.getPrototypeOf(chunk) !== Buffer.prototype) {\n chunk = _uint8ArrayToBuffer(chunk);\n }\n\n if (addToFront) {\n if (state.endEmitted) stream.emit('error', new Error('stream.unshift() after end event'));else addChunk(stream, state, chunk, true);\n } else if (state.ended) {\n stream.emit('error', new Error('stream.push() after EOF'));\n } else {\n state.reading = false;\n if (state.decoder && !encoding) {\n chunk = state.decoder.write(chunk);\n if (state.objectMode || chunk.length !== 0) addChunk(stream, state, chunk, false);else maybeReadMore(stream, state);\n } else {\n addChunk(stream, state, chunk, false);\n }\n }\n } else if (!addToFront) {\n state.reading = false;\n }\n }\n\n return needMoreData(state);\n}\n\nfunction addChunk(stream, state, chunk, addToFront) {\n if (state.flowing && state.length === 0 && !state.sync) {\n stream.emit('data', chunk);\n stream.read(0);\n } else {\n // update the buffer info.\n state.length += state.objectMode ? 1 : chunk.length;\n if (addToFront) state.buffer.unshift(chunk);else state.buffer.push(chunk);\n\n if (state.needReadable) emitReadable(stream);\n }\n maybeReadMore(stream, state);\n}\n\nfunction chunkInvalid(state, chunk) {\n var er;\n if (!_isUint8Array(chunk) && typeof chunk !== 'string' && chunk !== undefined && !state.objectMode) {\n er = new TypeError('Invalid non-string/buffer chunk');\n }\n return er;\n}\n\n// if it's past the high water mark, we can push in some more.\n// Also, if we have no data yet, we can stand some\n// more bytes. This is to work around cases where hwm=0,\n// such as the repl. Also, if the push() triggered a\n// readable event, and the user called read(largeNumber) such that\n// needReadable was set, then we ought to push more, so that another\n// 'readable' event will be triggered.\nfunction needMoreData(state) {\n return !state.ended && (state.needReadable || state.length < state.highWaterMark || state.length === 0);\n}\n\nReadable.prototype.isPaused = function () {\n return this._readableState.flowing === false;\n};\n\n// backwards compatibility.\nReadable.prototype.setEncoding = function (enc) {\n if (!StringDecoder) StringDecoder = __webpack_require__(/*! string_decoder/ */ \"./node_modules/string_decoder/lib/string_decoder.js\").StringDecoder;\n this._readableState.decoder = new StringDecoder(enc);\n this._readableState.encoding = enc;\n return this;\n};\n\n// Don't raise the hwm > 8MB\nvar MAX_HWM = 0x800000;\nfunction computeNewHighWaterMark(n) {\n if (n >= MAX_HWM) {\n n = MAX_HWM;\n } else {\n // Get the next highest power of 2 to prevent increasing hwm excessively in\n // tiny amounts\n n--;\n n |= n >>> 1;\n n |= n >>> 2;\n n |= n >>> 4;\n n |= n >>> 8;\n n |= n >>> 16;\n n++;\n }\n return n;\n}\n\n// This function is designed to be inlinable, so please take care when making\n// changes to the function body.\nfunction howMuchToRead(n, state) {\n if (n <= 0 || state.length === 0 && state.ended) return 0;\n if (state.objectMode) return 1;\n if (n !== n) {\n // Only flow one buffer at a time\n if (state.flowing && state.length) return state.buffer.head.data.length;else return state.length;\n }\n // If we're asking for more than the current hwm, then raise the hwm.\n if (n > state.highWaterMark) state.highWaterMark = computeNewHighWaterMark(n);\n if (n <= state.length) return n;\n // Don't have enough\n if (!state.ended) {\n state.needReadable = true;\n return 0;\n }\n return state.length;\n}\n\n// you can override either this method, or the async _read(n) below.\nReadable.prototype.read = function (n) {\n debug('read', n);\n n = parseInt(n, 10);\n var state = this._readableState;\n var nOrig = n;\n\n if (n !== 0) state.emittedReadable = false;\n\n // if we're doing read(0) to trigger a readable event, but we\n // already have a bunch of data in the buffer, then just trigger\n // the 'readable' event and move on.\n if (n === 0 && state.needReadable && (state.length >= state.highWaterMark || state.ended)) {\n debug('read: emitReadable', state.length, state.ended);\n if (state.length === 0 && state.ended) endReadable(this);else emitReadable(this);\n return null;\n }\n\n n = howMuchToRead(n, state);\n\n // if we've ended, and we're now clear, then finish it up.\n if (n === 0 && state.ended) {\n if (state.length === 0) endReadable(this);\n return null;\n }\n\n // All the actual chunk generation logic needs to be\n // *below* the call to _read. The reason is that in certain\n // synthetic stream cases, such as passthrough streams, _read\n // may be a completely synchronous operation which may change\n // the state of the read buffer, providing enough data when\n // before there was *not* enough.\n //\n // So, the steps are:\n // 1. Figure out what the state of things will be after we do\n // a read from the buffer.\n //\n // 2. If that resulting state will trigger a _read, then call _read.\n // Note that this may be asynchronous, or synchronous. Yes, it is\n // deeply ugly to write APIs this way, but that still doesn't mean\n // that the Readable class should behave improperly, as streams are\n // designed to be sync/async agnostic.\n // Take note if the _read call is sync or async (ie, if the read call\n // has returned yet), so that we know whether or not it's safe to emit\n // 'readable' etc.\n //\n // 3. Actually pull the requested chunks out of the buffer and return.\n\n // if we need a readable event, then we need to do some reading.\n var doRead = state.needReadable;\n debug('need readable', doRead);\n\n // if we currently have less than the highWaterMark, then also read some\n if (state.length === 0 || state.length - n < state.highWaterMark) {\n doRead = true;\n debug('length less than watermark', doRead);\n }\n\n // however, if we've ended, then there's no point, and if we're already\n // reading, then it's unnecessary.\n if (state.ended || state.reading) {\n doRead = false;\n debug('reading or ended', doRead);\n } else if (doRead) {\n debug('do read');\n state.reading = true;\n state.sync = true;\n // if the length is currently zero, then we *need* a readable event.\n if (state.length === 0) state.needReadable = true;\n // call internal read method\n this._read(state.highWaterMark);\n state.sync = false;\n // If _read pushed data synchronously, then `reading` will be false,\n // and we need to re-evaluate how much data we can return to the user.\n if (!state.reading) n = howMuchToRead(nOrig, state);\n }\n\n var ret;\n if (n > 0) ret = fromList(n, state);else ret = null;\n\n if (ret === null) {\n state.needReadable = true;\n n = 0;\n } else {\n state.length -= n;\n }\n\n if (state.length === 0) {\n // If we have nothing in the buffer, then we want to know\n // as soon as we *do* get something into the buffer.\n if (!state.ended) state.needReadable = true;\n\n // If we tried to read() past the EOF, then emit end on the next tick.\n if (nOrig !== n && state.ended) endReadable(this);\n }\n\n if (ret !== null) this.emit('data', ret);\n\n return ret;\n};\n\nfunction onEofChunk(stream, state) {\n if (state.ended) return;\n if (state.decoder) {\n var chunk = state.decoder.end();\n if (chunk && chunk.length) {\n state.buffer.push(chunk);\n state.length += state.objectMode ? 1 : chunk.length;\n }\n }\n state.ended = true;\n\n // emit 'readable' now to make sure it gets picked up.\n emitReadable(stream);\n}\n\n// Don't emit readable right away in sync mode, because this can trigger\n// another read() call => stack overflow. This way, it might trigger\n// a nextTick recursion warning, but that's not so bad.\nfunction emitReadable(stream) {\n var state = stream._readableState;\n state.needReadable = false;\n if (!state.emittedReadable) {\n debug('emitReadable', state.flowing);\n state.emittedReadable = true;\n if (state.sync) pna.nextTick(emitReadable_, stream);else emitReadable_(stream);\n }\n}\n\nfunction emitReadable_(stream) {\n debug('emit readable');\n stream.emit('readable');\n flow(stream);\n}\n\n// at this point, the user has presumably seen the 'readable' event,\n// and called read() to consume some data. that may have triggered\n// in turn another _read(n) call, in which case reading = true if\n// it's in progress.\n// However, if we're not ended, or reading, and the length < hwm,\n// then go ahead and try to read some more preemptively.\nfunction maybeReadMore(stream, state) {\n if (!state.readingMore) {\n state.readingMore = true;\n pna.nextTick(maybeReadMore_, stream, state);\n }\n}\n\nfunction maybeReadMore_(stream, state) {\n var len = state.length;\n while (!state.reading && !state.flowing && !state.ended && state.length < state.highWaterMark) {\n debug('maybeReadMore read 0');\n stream.read(0);\n if (len === state.length)\n // didn't get any data, stop spinning.\n break;else len = state.length;\n }\n state.readingMore = false;\n}\n\n// abstract method. to be overridden in specific implementation classes.\n// call cb(er, data) where data is <= n in length.\n// for virtual (non-string, non-buffer) streams, \"length\" is somewhat\n// arbitrary, and perhaps not very meaningful.\nReadable.prototype._read = function (n) {\n this.emit('error', new Error('_read() is not implemented'));\n};\n\nReadable.prototype.pipe = function (dest, pipeOpts) {\n var src = this;\n var state = this._readableState;\n\n switch (state.pipesCount) {\n case 0:\n state.pipes = dest;\n break;\n case 1:\n state.pipes = [state.pipes, dest];\n break;\n default:\n state.pipes.push(dest);\n break;\n }\n state.pipesCount += 1;\n debug('pipe count=%d opts=%j', state.pipesCount, pipeOpts);\n\n var doEnd = (!pipeOpts || pipeOpts.end !== false) && dest !== process.stdout && dest !== process.stderr;\n\n var endFn = doEnd ? onend : unpipe;\n if (state.endEmitted) pna.nextTick(endFn);else src.once('end', endFn);\n\n dest.on('unpipe', onunpipe);\n function onunpipe(readable, unpipeInfo) {\n debug('onunpipe');\n if (readable === src) {\n if (unpipeInfo && unpipeInfo.hasUnpiped === false) {\n unpipeInfo.hasUnpiped = true;\n cleanup();\n }\n }\n }\n\n function onend() {\n debug('onend');\n dest.end();\n }\n\n // when the dest drains, it reduces the awaitDrain counter\n // on the source. This would be more elegant with a .once()\n // handler in flow(), but adding and removing repeatedly is\n // too slow.\n var ondrain = pipeOnDrain(src);\n dest.on('drain', ondrain);\n\n var cleanedUp = false;\n function cleanup() {\n debug('cleanup');\n // cleanup event handlers once the pipe is broken\n dest.removeListener('close', onclose);\n dest.removeListener('finish', onfinish);\n dest.removeListener('drain', ondrain);\n dest.removeListener('error', onerror);\n dest.removeListener('unpipe', onunpipe);\n src.removeListener('end', onend);\n src.removeListener('end', unpipe);\n src.removeListener('data', ondata);\n\n cleanedUp = true;\n\n // if the reader is waiting for a drain event from this\n // specific writer, then it would cause it to never start\n // flowing again.\n // So, if this is awaiting a drain, then we just call it now.\n // If we don't know, then assume that we are waiting for one.\n if (state.awaitDrain && (!dest._writableState || dest._writableState.needDrain)) ondrain();\n }\n\n // If the user pushes more data while we're writing to dest then we'll end up\n // in ondata again. However, we only want to increase awaitDrain once because\n // dest will only emit one 'drain' event for the multiple writes.\n // => Introduce a guard on increasing awaitDrain.\n var increasedAwaitDrain = false;\n src.on('data', ondata);\n function ondata(chunk) {\n debug('ondata');\n increasedAwaitDrain = false;\n var ret = dest.write(chunk);\n if (false === ret && !increasedAwaitDrain) {\n // If the user unpiped during `dest.write()`, it is possible\n // to get stuck in a permanently paused state if that write\n // also returned false.\n // => Check whether `dest` is still a piping destination.\n if ((state.pipesCount === 1 && state.pipes === dest || state.pipesCount > 1 && indexOf(state.pipes, dest) !== -1) && !cleanedUp) {\n debug('false write response, pause', src._readableState.awaitDrain);\n src._readableState.awaitDrain++;\n increasedAwaitDrain = true;\n }\n src.pause();\n }\n }\n\n // if the dest has an error, then stop piping into it.\n // however, don't suppress the throwing behavior for this.\n function onerror(er) {\n debug('onerror', er);\n unpipe();\n dest.removeListener('error', onerror);\n if (EElistenerCount(dest, 'error') === 0) dest.emit('error', er);\n }\n\n // Make sure our error handler is attached before userland ones.\n prependListener(dest, 'error', onerror);\n\n // Both close and finish should trigger unpipe, but only once.\n function onclose() {\n dest.removeListener('finish', onfinish);\n unpipe();\n }\n dest.once('close', onclose);\n function onfinish() {\n debug('onfinish');\n dest.removeListener('close', onclose);\n unpipe();\n }\n dest.once('finish', onfinish);\n\n function unpipe() {\n debug('unpipe');\n src.unpipe(dest);\n }\n\n // tell the dest that it's being piped to\n dest.emit('pipe', src);\n\n // start the flow if it hasn't been started already.\n if (!state.flowing) {\n debug('pipe resume');\n src.resume();\n }\n\n return dest;\n};\n\nfunction pipeOnDrain(src) {\n return function () {\n var state = src._readableState;\n debug('pipeOnDrain', state.awaitDrain);\n if (state.awaitDrain) state.awaitDrain--;\n if (state.awaitDrain === 0 && EElistenerCount(src, 'data')) {\n state.flowing = true;\n flow(src);\n }\n };\n}\n\nReadable.prototype.unpipe = function (dest) {\n var state = this._readableState;\n var unpipeInfo = { hasUnpiped: false };\n\n // if we're not piping anywhere, then do nothing.\n if (state.pipesCount === 0) return this;\n\n // just one destination. most common case.\n if (state.pipesCount === 1) {\n // passed in one, but it's not the right one.\n if (dest && dest !== state.pipes) return this;\n\n if (!dest) dest = state.pipes;\n\n // got a match.\n state.pipes = null;\n state.pipesCount = 0;\n state.flowing = false;\n if (dest) dest.emit('unpipe', this, unpipeInfo);\n return this;\n }\n\n // slow case. multiple pipe destinations.\n\n if (!dest) {\n // remove all.\n var dests = state.pipes;\n var len = state.pipesCount;\n state.pipes = null;\n state.pipesCount = 0;\n state.flowing = false;\n\n for (var i = 0; i < len; i++) {\n dests[i].emit('unpipe', this, unpipeInfo);\n }return this;\n }\n\n // try to find the right one.\n var index = indexOf(state.pipes, dest);\n if (index === -1) return this;\n\n state.pipes.splice(index, 1);\n state.pipesCount -= 1;\n if (state.pipesCount === 1) state.pipes = state.pipes[0];\n\n dest.emit('unpipe', this, unpipeInfo);\n\n return this;\n};\n\n// set up data events if they are asked for\n// Ensure readable listeners eventually get something\nReadable.prototype.on = function (ev, fn) {\n var res = Stream.prototype.on.call(this, ev, fn);\n\n if (ev === 'data') {\n // Start flowing on next tick if stream isn't explicitly paused\n if (this._readableState.flowing !== false) this.resume();\n } else if (ev === 'readable') {\n var state = this._readableState;\n if (!state.endEmitted && !state.readableListening) {\n state.readableListening = state.needReadable = true;\n state.emittedReadable = false;\n if (!state.reading) {\n pna.nextTick(nReadingNextTick, this);\n } else if (state.length) {\n emitReadable(this);\n }\n }\n }\n\n return res;\n};\nReadable.prototype.addListener = Readable.prototype.on;\n\nfunction nReadingNextTick(self) {\n debug('readable nexttick read 0');\n self.read(0);\n}\n\n// pause() and resume() are remnants of the legacy readable stream API\n// If the user uses them, then switch into old mode.\nReadable.prototype.resume = function () {\n var state = this._readableState;\n if (!state.flowing) {\n debug('resume');\n state.flowing = true;\n resume(this, state);\n }\n return this;\n};\n\nfunction resume(stream, state) {\n if (!state.resumeScheduled) {\n state.resumeScheduled = true;\n pna.nextTick(resume_, stream, state);\n }\n}\n\nfunction resume_(stream, state) {\n if (!state.reading) {\n debug('resume read 0');\n stream.read(0);\n }\n\n state.resumeScheduled = false;\n state.awaitDrain = 0;\n stream.emit('resume');\n flow(stream);\n if (state.flowing && !state.reading) stream.read(0);\n}\n\nReadable.prototype.pause = function () {\n debug('call pause flowing=%j', this._readableState.flowing);\n if (false !== this._readableState.flowing) {\n debug('pause');\n this._readableState.flowing = false;\n this.emit('pause');\n }\n return this;\n};\n\nfunction flow(stream) {\n var state = stream._readableState;\n debug('flow', state.flowing);\n while (state.flowing && stream.read() !== null) {}\n}\n\n// wrap an old-style stream as the async data source.\n// This is *not* part of the readable stream interface.\n// It is an ugly unfortunate mess of history.\nReadable.prototype.wrap = function (stream) {\n var _this = this;\n\n var state = this._readableState;\n var paused = false;\n\n stream.on('end', function () {\n debug('wrapped end');\n if (state.decoder && !state.ended) {\n var chunk = state.decoder.end();\n if (chunk && chunk.length) _this.push(chunk);\n }\n\n _this.push(null);\n });\n\n stream.on('data', function (chunk) {\n debug('wrapped data');\n if (state.decoder) chunk = state.decoder.write(chunk);\n\n // don't skip over falsy values in objectMode\n if (state.objectMode && (chunk === null || chunk === undefined)) return;else if (!state.objectMode && (!chunk || !chunk.length)) return;\n\n var ret = _this.push(chunk);\n if (!ret) {\n paused = true;\n stream.pause();\n }\n });\n\n // proxy all the other methods.\n // important when wrapping filters and duplexes.\n for (var i in stream) {\n if (this[i] === undefined && typeof stream[i] === 'function') {\n this[i] = function (method) {\n return function () {\n return stream[method].apply(stream, arguments);\n };\n }(i);\n }\n }\n\n // proxy certain important events.\n for (var n = 0; n < kProxyEvents.length; n++) {\n stream.on(kProxyEvents[n], this.emit.bind(this, kProxyEvents[n]));\n }\n\n // when we try to consume some more bytes, simply unpause the\n // underlying stream.\n this._read = function (n) {\n debug('wrapped _read', n);\n if (paused) {\n paused = false;\n stream.resume();\n }\n };\n\n return this;\n};\n\nObject.defineProperty(Readable.prototype, 'readableHighWaterMark', {\n // making it explicit this property is not enumerable\n // because otherwise some prototype manipulation in\n // userland will fail\n enumerable: false,\n get: function () {\n return this._readableState.highWaterMark;\n }\n});\n\n// exposed for testing purposes only.\nReadable._fromList = fromList;\n\n// Pluck off n bytes from an array of buffers.\n// Length is the combined lengths of all the buffers in the list.\n// This function is designed to be inlinable, so please take care when making\n// changes to the function body.\nfunction fromList(n, state) {\n // nothing buffered\n if (state.length === 0) return null;\n\n var ret;\n if (state.objectMode) ret = state.buffer.shift();else if (!n || n >= state.length) {\n // read it all, truncate the list\n if (state.decoder) ret = state.buffer.join('');else if (state.buffer.length === 1) ret = state.buffer.head.data;else ret = state.buffer.concat(state.length);\n state.buffer.clear();\n } else {\n // read part of list\n ret = fromListPartial(n, state.buffer, state.decoder);\n }\n\n return ret;\n}\n\n// Extracts only enough buffered data to satisfy the amount requested.\n// This function is designed to be inlinable, so please take care when making\n// changes to the function body.\nfunction fromListPartial(n, list, hasStrings) {\n var ret;\n if (n < list.head.data.length) {\n // slice is the same for buffers and strings\n ret = list.head.data.slice(0, n);\n list.head.data = list.head.data.slice(n);\n } else if (n === list.head.data.length) {\n // first chunk is a perfect match\n ret = list.shift();\n } else {\n // result spans more than one buffer\n ret = hasStrings ? copyFromBufferString(n, list) : copyFromBuffer(n, list);\n }\n return ret;\n}\n\n// Copies a specified amount of characters from the list of buffered data\n// chunks.\n// This function is designed to be inlinable, so please take care when making\n// changes to the function body.\nfunction copyFromBufferString(n, list) {\n var p = list.head;\n var c = 1;\n var ret = p.data;\n n -= ret.length;\n while (p = p.next) {\n var str = p.data;\n var nb = n > str.length ? str.length : n;\n if (nb === str.length) ret += str;else ret += str.slice(0, n);\n n -= nb;\n if (n === 0) {\n if (nb === str.length) {\n ++c;\n if (p.next) list.head = p.next;else list.head = list.tail = null;\n } else {\n list.head = p;\n p.data = str.slice(nb);\n }\n break;\n }\n ++c;\n }\n list.length -= c;\n return ret;\n}\n\n// Copies a specified amount of bytes from the list of buffered data chunks.\n// This function is designed to be inlinable, so please take care when making\n// changes to the function body.\nfunction copyFromBuffer(n, list) {\n var ret = Buffer.allocUnsafe(n);\n var p = list.head;\n var c = 1;\n p.data.copy(ret);\n n -= p.data.length;\n while (p = p.next) {\n var buf = p.data;\n var nb = n > buf.length ? buf.length : n;\n buf.copy(ret, ret.length - n, 0, nb);\n n -= nb;\n if (n === 0) {\n if (nb === buf.length) {\n ++c;\n if (p.next) list.head = p.next;else list.head = list.tail = null;\n } else {\n list.head = p;\n p.data = buf.slice(nb);\n }\n break;\n }\n ++c;\n }\n list.length -= c;\n return ret;\n}\n\nfunction endReadable(stream) {\n var state = stream._readableState;\n\n // If we get here before consuming all the bytes, then that is a\n // bug in node. Should never happen.\n if (state.length > 0) throw new Error('\"endReadable()\" called on non-empty stream');\n\n if (!state.endEmitted) {\n state.ended = true;\n pna.nextTick(endReadableNT, state, stream);\n }\n}\n\nfunction endReadableNT(state, stream) {\n // Check that we didn't get one last unshift.\n if (!state.endEmitted && state.length === 0) {\n state.endEmitted = true;\n stream.readable = false;\n stream.emit('end');\n }\n}\n\nfunction indexOf(xs, x) {\n for (var i = 0, l = xs.length; i < l; i++) {\n if (xs[i] === x) return i;\n }\n return -1;\n}\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/_stream_readable.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/_stream_transform.js": -/*!***************************************************************!*\ - !*** ./node_modules/readable-stream/lib/_stream_transform.js ***! - \***************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n// a transform stream is a readable/writable stream where you do\n// something with the data. Sometimes it's called a \"filter\",\n// but that's not a great name for it, since that implies a thing where\n// some bits pass through, and others are simply ignored. (That would\n// be a valid example of a transform, of course.)\n//\n// While the output is causally related to the input, it's not a\n// necessarily symmetric or synchronous transformation. For example,\n// a zlib stream might take multiple plain-text writes(), and then\n// emit a single compressed chunk some time in the future.\n//\n// Here's how this works:\n//\n// The Transform stream has all the aspects of the readable and writable\n// stream classes. When you write(chunk), that calls _write(chunk,cb)\n// internally, and returns false if there's a lot of pending writes\n// buffered up. When you call read(), that calls _read(n) until\n// there's enough pending readable data buffered up.\n//\n// In a transform stream, the written data is placed in a buffer. When\n// _read(n) is called, it transforms the queued up data, calling the\n// buffered _write cb's as it consumes chunks. If consuming a single\n// written chunk would result in multiple output chunks, then the first\n// outputted bit calls the readcb, and subsequent chunks just go into\n// the read buffer, and will cause it to emit 'readable' if necessary.\n//\n// This way, back-pressure is actually determined by the reading side,\n// since _read has to be called to start processing a new chunk. However,\n// a pathological inflate type of transform can cause excessive buffering\n// here. For example, imagine a stream where every byte of input is\n// interpreted as an integer from 0-255, and then results in that many\n// bytes of output. Writing the 4 bytes {ff,ff,ff,ff} would result in\n// 1kb of data being output. In this case, you could write a very small\n// amount of input, and end up with a very large amount of output. In\n// such a pathological inflating mechanism, there'd be no way to tell\n// the system to stop doing the transform. A single 4MB write could\n// cause the system to run out of memory.\n//\n// However, even in such a pathological case, only a single written chunk\n// would be consumed, and then the rest would wait (un-transformed) until\n// the results of the previous transformed chunk were consumed.\n\n\n\nmodule.exports = Transform;\n\nvar Duplex = __webpack_require__(/*! ./_stream_duplex */ \"./node_modules/readable-stream/lib/_stream_duplex.js\");\n\n/**/\nvar util = Object.create(__webpack_require__(/*! core-util-is */ \"./node_modules/core-util-is/lib/util.js\"));\nutil.inherits = __webpack_require__(/*! inherits */ \"./node_modules/inherits/inherits.js\");\n/**/\n\nutil.inherits(Transform, Duplex);\n\nfunction afterTransform(er, data) {\n var ts = this._transformState;\n ts.transforming = false;\n\n var cb = ts.writecb;\n\n if (!cb) {\n return this.emit('error', new Error('write callback called multiple times'));\n }\n\n ts.writechunk = null;\n ts.writecb = null;\n\n if (data != null) // single equals check for both `null` and `undefined`\n this.push(data);\n\n cb(er);\n\n var rs = this._readableState;\n rs.reading = false;\n if (rs.needReadable || rs.length < rs.highWaterMark) {\n this._read(rs.highWaterMark);\n }\n}\n\nfunction Transform(options) {\n if (!(this instanceof Transform)) return new Transform(options);\n\n Duplex.call(this, options);\n\n this._transformState = {\n afterTransform: afterTransform.bind(this),\n needTransform: false,\n transforming: false,\n writecb: null,\n writechunk: null,\n writeencoding: null\n };\n\n // start out asking for a readable event once data is transformed.\n this._readableState.needReadable = true;\n\n // we have implemented the _read method, and done the other things\n // that Readable wants before the first _read call, so unset the\n // sync guard flag.\n this._readableState.sync = false;\n\n if (options) {\n if (typeof options.transform === 'function') this._transform = options.transform;\n\n if (typeof options.flush === 'function') this._flush = options.flush;\n }\n\n // When the writable side finishes, then flush out anything remaining.\n this.on('prefinish', prefinish);\n}\n\nfunction prefinish() {\n var _this = this;\n\n if (typeof this._flush === 'function') {\n this._flush(function (er, data) {\n done(_this, er, data);\n });\n } else {\n done(this, null, null);\n }\n}\n\nTransform.prototype.push = function (chunk, encoding) {\n this._transformState.needTransform = false;\n return Duplex.prototype.push.call(this, chunk, encoding);\n};\n\n// This is the part where you do stuff!\n// override this function in implementation classes.\n// 'chunk' is an input chunk.\n//\n// Call `push(newChunk)` to pass along transformed output\n// to the readable side. You may call 'push' zero or more times.\n//\n// Call `cb(err)` when you are done with this chunk. If you pass\n// an error, then that'll put the hurt on the whole operation. If you\n// never call cb(), then you'll never get another chunk.\nTransform.prototype._transform = function (chunk, encoding, cb) {\n throw new Error('_transform() is not implemented');\n};\n\nTransform.prototype._write = function (chunk, encoding, cb) {\n var ts = this._transformState;\n ts.writecb = cb;\n ts.writechunk = chunk;\n ts.writeencoding = encoding;\n if (!ts.transforming) {\n var rs = this._readableState;\n if (ts.needTransform || rs.needReadable || rs.length < rs.highWaterMark) this._read(rs.highWaterMark);\n }\n};\n\n// Doesn't matter what the args are here.\n// _transform does all the work.\n// That we got here means that the readable side wants more data.\nTransform.prototype._read = function (n) {\n var ts = this._transformState;\n\n if (ts.writechunk !== null && ts.writecb && !ts.transforming) {\n ts.transforming = true;\n this._transform(ts.writechunk, ts.writeencoding, ts.afterTransform);\n } else {\n // mark that we need a transform, so that any data that comes in\n // will get processed, now that we've asked for it.\n ts.needTransform = true;\n }\n};\n\nTransform.prototype._destroy = function (err, cb) {\n var _this2 = this;\n\n Duplex.prototype._destroy.call(this, err, function (err2) {\n cb(err2);\n _this2.emit('close');\n });\n};\n\nfunction done(stream, er, data) {\n if (er) return stream.emit('error', er);\n\n if (data != null) // single equals check for both `null` and `undefined`\n stream.push(data);\n\n // if there's nothing in the write buffer, then that means\n // that nothing more will ever be provided\n if (stream._writableState.length) throw new Error('Calling transform done when ws.length != 0');\n\n if (stream._transformState.transforming) throw new Error('Calling transform done when still transforming');\n\n return stream.push(null);\n}\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/_stream_transform.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/_stream_writable.js": -/*!**************************************************************!*\ - !*** ./node_modules/readable-stream/lib/_stream_writable.js ***! - \**************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n// A bit simpler than readable streams.\n// Implement an async ._write(chunk, encoding, cb), and it'll handle all\n// the drain event emission and buffering.\n\n\n\n/**/\n\nvar pna = __webpack_require__(/*! process-nextick-args */ \"./node_modules/process-nextick-args/index.js\");\n/**/\n\nmodule.exports = Writable;\n\n/* */\nfunction WriteReq(chunk, encoding, cb) {\n this.chunk = chunk;\n this.encoding = encoding;\n this.callback = cb;\n this.next = null;\n}\n\n// It seems a linked list but it is not\n// there will be only 2 of these for each stream\nfunction CorkedRequest(state) {\n var _this = this;\n\n this.next = null;\n this.entry = null;\n this.finish = function () {\n onCorkedFinish(_this, state);\n };\n}\n/* */\n\n/**/\nvar asyncWrite = !process.browser && ['v0.10', 'v0.9.'].indexOf(process.version.slice(0, 5)) > -1 ? setImmediate : pna.nextTick;\n/**/\n\n/**/\nvar Duplex;\n/**/\n\nWritable.WritableState = WritableState;\n\n/**/\nvar util = Object.create(__webpack_require__(/*! core-util-is */ \"./node_modules/core-util-is/lib/util.js\"));\nutil.inherits = __webpack_require__(/*! inherits */ \"./node_modules/inherits/inherits.js\");\n/**/\n\n/**/\nvar internalUtil = {\n deprecate: __webpack_require__(/*! util-deprecate */ \"./node_modules/util-deprecate/node.js\")\n};\n/**/\n\n/**/\nvar Stream = __webpack_require__(/*! ./internal/streams/stream */ \"./node_modules/readable-stream/lib/internal/streams/stream.js\");\n/**/\n\n/**/\n\nvar Buffer = __webpack_require__(/*! safe-buffer */ \"./node_modules/safe-buffer/index.js\").Buffer;\nvar OurUint8Array = global.Uint8Array || function () {};\nfunction _uint8ArrayToBuffer(chunk) {\n return Buffer.from(chunk);\n}\nfunction _isUint8Array(obj) {\n return Buffer.isBuffer(obj) || obj instanceof OurUint8Array;\n}\n\n/**/\n\nvar destroyImpl = __webpack_require__(/*! ./internal/streams/destroy */ \"./node_modules/readable-stream/lib/internal/streams/destroy.js\");\n\nutil.inherits(Writable, Stream);\n\nfunction nop() {}\n\nfunction WritableState(options, stream) {\n Duplex = Duplex || __webpack_require__(/*! ./_stream_duplex */ \"./node_modules/readable-stream/lib/_stream_duplex.js\");\n\n options = options || {};\n\n // Duplex streams are both readable and writable, but share\n // the same options object.\n // However, some cases require setting options to different\n // values for the readable and the writable sides of the duplex stream.\n // These options can be provided separately as readableXXX and writableXXX.\n var isDuplex = stream instanceof Duplex;\n\n // object stream flag to indicate whether or not this stream\n // contains buffers or objects.\n this.objectMode = !!options.objectMode;\n\n if (isDuplex) this.objectMode = this.objectMode || !!options.writableObjectMode;\n\n // the point at which write() starts returning false\n // Note: 0 is a valid value, means that we always return false if\n // the entire buffer is not flushed immediately on write()\n var hwm = options.highWaterMark;\n var writableHwm = options.writableHighWaterMark;\n var defaultHwm = this.objectMode ? 16 : 16 * 1024;\n\n if (hwm || hwm === 0) this.highWaterMark = hwm;else if (isDuplex && (writableHwm || writableHwm === 0)) this.highWaterMark = writableHwm;else this.highWaterMark = defaultHwm;\n\n // cast to ints.\n this.highWaterMark = Math.floor(this.highWaterMark);\n\n // if _final has been called\n this.finalCalled = false;\n\n // drain event flag.\n this.needDrain = false;\n // at the start of calling end()\n this.ending = false;\n // when end() has been called, and returned\n this.ended = false;\n // when 'finish' is emitted\n this.finished = false;\n\n // has it been destroyed\n this.destroyed = false;\n\n // should we decode strings into buffers before passing to _write?\n // this is here so that some node-core streams can optimize string\n // handling at a lower level.\n var noDecode = options.decodeStrings === false;\n this.decodeStrings = !noDecode;\n\n // Crypto is kind of old and crusty. Historically, its default string\n // encoding is 'binary' so we have to make this configurable.\n // Everything else in the universe uses 'utf8', though.\n this.defaultEncoding = options.defaultEncoding || 'utf8';\n\n // not an actual buffer we keep track of, but a measurement\n // of how much we're waiting to get pushed to some underlying\n // socket or file.\n this.length = 0;\n\n // a flag to see when we're in the middle of a write.\n this.writing = false;\n\n // when true all writes will be buffered until .uncork() call\n this.corked = 0;\n\n // a flag to be able to tell if the onwrite cb is called immediately,\n // or on a later tick. We set this to true at first, because any\n // actions that shouldn't happen until \"later\" should generally also\n // not happen before the first write call.\n this.sync = true;\n\n // a flag to know if we're processing previously buffered items, which\n // may call the _write() callback in the same tick, so that we don't\n // end up in an overlapped onwrite situation.\n this.bufferProcessing = false;\n\n // the callback that's passed to _write(chunk,cb)\n this.onwrite = function (er) {\n onwrite(stream, er);\n };\n\n // the callback that the user supplies to write(chunk,encoding,cb)\n this.writecb = null;\n\n // the amount that is being written when _write is called.\n this.writelen = 0;\n\n this.bufferedRequest = null;\n this.lastBufferedRequest = null;\n\n // number of pending user-supplied write callbacks\n // this must be 0 before 'finish' can be emitted\n this.pendingcb = 0;\n\n // emit prefinish if the only thing we're waiting for is _write cbs\n // This is relevant for synchronous Transform streams\n this.prefinished = false;\n\n // True if the error was already emitted and should not be thrown again\n this.errorEmitted = false;\n\n // count buffered requests\n this.bufferedRequestCount = 0;\n\n // allocate the first CorkedRequest, there is always\n // one allocated and free to use, and we maintain at most two\n this.corkedRequestsFree = new CorkedRequest(this);\n}\n\nWritableState.prototype.getBuffer = function getBuffer() {\n var current = this.bufferedRequest;\n var out = [];\n while (current) {\n out.push(current);\n current = current.next;\n }\n return out;\n};\n\n(function () {\n try {\n Object.defineProperty(WritableState.prototype, 'buffer', {\n get: internalUtil.deprecate(function () {\n return this.getBuffer();\n }, '_writableState.buffer is deprecated. Use _writableState.getBuffer ' + 'instead.', 'DEP0003')\n });\n } catch (_) {}\n})();\n\n// Test _writableState for inheritance to account for Duplex streams,\n// whose prototype chain only points to Readable.\nvar realHasInstance;\nif (typeof Symbol === 'function' && Symbol.hasInstance && typeof Function.prototype[Symbol.hasInstance] === 'function') {\n realHasInstance = Function.prototype[Symbol.hasInstance];\n Object.defineProperty(Writable, Symbol.hasInstance, {\n value: function (object) {\n if (realHasInstance.call(this, object)) return true;\n if (this !== Writable) return false;\n\n return object && object._writableState instanceof WritableState;\n }\n });\n} else {\n realHasInstance = function (object) {\n return object instanceof this;\n };\n}\n\nfunction Writable(options) {\n Duplex = Duplex || __webpack_require__(/*! ./_stream_duplex */ \"./node_modules/readable-stream/lib/_stream_duplex.js\");\n\n // Writable ctor is applied to Duplexes, too.\n // `realHasInstance` is necessary because using plain `instanceof`\n // would return false, as no `_writableState` property is attached.\n\n // Trying to use the custom `instanceof` for Writable here will also break the\n // Node.js LazyTransform implementation, which has a non-trivial getter for\n // `_writableState` that would lead to infinite recursion.\n if (!realHasInstance.call(Writable, this) && !(this instanceof Duplex)) {\n return new Writable(options);\n }\n\n this._writableState = new WritableState(options, this);\n\n // legacy.\n this.writable = true;\n\n if (options) {\n if (typeof options.write === 'function') this._write = options.write;\n\n if (typeof options.writev === 'function') this._writev = options.writev;\n\n if (typeof options.destroy === 'function') this._destroy = options.destroy;\n\n if (typeof options.final === 'function') this._final = options.final;\n }\n\n Stream.call(this);\n}\n\n// Otherwise people can pipe Writable streams, which is just wrong.\nWritable.prototype.pipe = function () {\n this.emit('error', new Error('Cannot pipe, not readable'));\n};\n\nfunction writeAfterEnd(stream, cb) {\n var er = new Error('write after end');\n // TODO: defer error events consistently everywhere, not just the cb\n stream.emit('error', er);\n pna.nextTick(cb, er);\n}\n\n// Checks that a user-supplied chunk is valid, especially for the particular\n// mode the stream is in. Currently this means that `null` is never accepted\n// and undefined/non-string values are only allowed in object mode.\nfunction validChunk(stream, state, chunk, cb) {\n var valid = true;\n var er = false;\n\n if (chunk === null) {\n er = new TypeError('May not write null values to stream');\n } else if (typeof chunk !== 'string' && chunk !== undefined && !state.objectMode) {\n er = new TypeError('Invalid non-string/buffer chunk');\n }\n if (er) {\n stream.emit('error', er);\n pna.nextTick(cb, er);\n valid = false;\n }\n return valid;\n}\n\nWritable.prototype.write = function (chunk, encoding, cb) {\n var state = this._writableState;\n var ret = false;\n var isBuf = !state.objectMode && _isUint8Array(chunk);\n\n if (isBuf && !Buffer.isBuffer(chunk)) {\n chunk = _uint8ArrayToBuffer(chunk);\n }\n\n if (typeof encoding === 'function') {\n cb = encoding;\n encoding = null;\n }\n\n if (isBuf) encoding = 'buffer';else if (!encoding) encoding = state.defaultEncoding;\n\n if (typeof cb !== 'function') cb = nop;\n\n if (state.ended) writeAfterEnd(this, cb);else if (isBuf || validChunk(this, state, chunk, cb)) {\n state.pendingcb++;\n ret = writeOrBuffer(this, state, isBuf, chunk, encoding, cb);\n }\n\n return ret;\n};\n\nWritable.prototype.cork = function () {\n var state = this._writableState;\n\n state.corked++;\n};\n\nWritable.prototype.uncork = function () {\n var state = this._writableState;\n\n if (state.corked) {\n state.corked--;\n\n if (!state.writing && !state.corked && !state.finished && !state.bufferProcessing && state.bufferedRequest) clearBuffer(this, state);\n }\n};\n\nWritable.prototype.setDefaultEncoding = function setDefaultEncoding(encoding) {\n // node::ParseEncoding() requires lower case.\n if (typeof encoding === 'string') encoding = encoding.toLowerCase();\n if (!(['hex', 'utf8', 'utf-8', 'ascii', 'binary', 'base64', 'ucs2', 'ucs-2', 'utf16le', 'utf-16le', 'raw'].indexOf((encoding + '').toLowerCase()) > -1)) throw new TypeError('Unknown encoding: ' + encoding);\n this._writableState.defaultEncoding = encoding;\n return this;\n};\n\nfunction decodeChunk(state, chunk, encoding) {\n if (!state.objectMode && state.decodeStrings !== false && typeof chunk === 'string') {\n chunk = Buffer.from(chunk, encoding);\n }\n return chunk;\n}\n\nObject.defineProperty(Writable.prototype, 'writableHighWaterMark', {\n // making it explicit this property is not enumerable\n // because otherwise some prototype manipulation in\n // userland will fail\n enumerable: false,\n get: function () {\n return this._writableState.highWaterMark;\n }\n});\n\n// if we're already writing something, then just put this\n// in the queue, and wait our turn. Otherwise, call _write\n// If we return false, then we need a drain event, so set that flag.\nfunction writeOrBuffer(stream, state, isBuf, chunk, encoding, cb) {\n if (!isBuf) {\n var newChunk = decodeChunk(state, chunk, encoding);\n if (chunk !== newChunk) {\n isBuf = true;\n encoding = 'buffer';\n chunk = newChunk;\n }\n }\n var len = state.objectMode ? 1 : chunk.length;\n\n state.length += len;\n\n var ret = state.length < state.highWaterMark;\n // we must ensure that previous needDrain will not be reset to false.\n if (!ret) state.needDrain = true;\n\n if (state.writing || state.corked) {\n var last = state.lastBufferedRequest;\n state.lastBufferedRequest = {\n chunk: chunk,\n encoding: encoding,\n isBuf: isBuf,\n callback: cb,\n next: null\n };\n if (last) {\n last.next = state.lastBufferedRequest;\n } else {\n state.bufferedRequest = state.lastBufferedRequest;\n }\n state.bufferedRequestCount += 1;\n } else {\n doWrite(stream, state, false, len, chunk, encoding, cb);\n }\n\n return ret;\n}\n\nfunction doWrite(stream, state, writev, len, chunk, encoding, cb) {\n state.writelen = len;\n state.writecb = cb;\n state.writing = true;\n state.sync = true;\n if (writev) stream._writev(chunk, state.onwrite);else stream._write(chunk, encoding, state.onwrite);\n state.sync = false;\n}\n\nfunction onwriteError(stream, state, sync, er, cb) {\n --state.pendingcb;\n\n if (sync) {\n // defer the callback if we are being called synchronously\n // to avoid piling up things on the stack\n pna.nextTick(cb, er);\n // this can emit finish, and it will always happen\n // after error\n pna.nextTick(finishMaybe, stream, state);\n stream._writableState.errorEmitted = true;\n stream.emit('error', er);\n } else {\n // the caller expect this to happen before if\n // it is async\n cb(er);\n stream._writableState.errorEmitted = true;\n stream.emit('error', er);\n // this can emit finish, but finish must\n // always follow error\n finishMaybe(stream, state);\n }\n}\n\nfunction onwriteStateUpdate(state) {\n state.writing = false;\n state.writecb = null;\n state.length -= state.writelen;\n state.writelen = 0;\n}\n\nfunction onwrite(stream, er) {\n var state = stream._writableState;\n var sync = state.sync;\n var cb = state.writecb;\n\n onwriteStateUpdate(state);\n\n if (er) onwriteError(stream, state, sync, er, cb);else {\n // Check if we're actually ready to finish, but don't emit yet\n var finished = needFinish(state);\n\n if (!finished && !state.corked && !state.bufferProcessing && state.bufferedRequest) {\n clearBuffer(stream, state);\n }\n\n if (sync) {\n /**/\n asyncWrite(afterWrite, stream, state, finished, cb);\n /**/\n } else {\n afterWrite(stream, state, finished, cb);\n }\n }\n}\n\nfunction afterWrite(stream, state, finished, cb) {\n if (!finished) onwriteDrain(stream, state);\n state.pendingcb--;\n cb();\n finishMaybe(stream, state);\n}\n\n// Must force callback to be called on nextTick, so that we don't\n// emit 'drain' before the write() consumer gets the 'false' return\n// value, and has a chance to attach a 'drain' listener.\nfunction onwriteDrain(stream, state) {\n if (state.length === 0 && state.needDrain) {\n state.needDrain = false;\n stream.emit('drain');\n }\n}\n\n// if there's something in the buffer waiting, then process it\nfunction clearBuffer(stream, state) {\n state.bufferProcessing = true;\n var entry = state.bufferedRequest;\n\n if (stream._writev && entry && entry.next) {\n // Fast case, write everything using _writev()\n var l = state.bufferedRequestCount;\n var buffer = new Array(l);\n var holder = state.corkedRequestsFree;\n holder.entry = entry;\n\n var count = 0;\n var allBuffers = true;\n while (entry) {\n buffer[count] = entry;\n if (!entry.isBuf) allBuffers = false;\n entry = entry.next;\n count += 1;\n }\n buffer.allBuffers = allBuffers;\n\n doWrite(stream, state, true, state.length, buffer, '', holder.finish);\n\n // doWrite is almost always async, defer these to save a bit of time\n // as the hot path ends with doWrite\n state.pendingcb++;\n state.lastBufferedRequest = null;\n if (holder.next) {\n state.corkedRequestsFree = holder.next;\n holder.next = null;\n } else {\n state.corkedRequestsFree = new CorkedRequest(state);\n }\n state.bufferedRequestCount = 0;\n } else {\n // Slow case, write chunks one-by-one\n while (entry) {\n var chunk = entry.chunk;\n var encoding = entry.encoding;\n var cb = entry.callback;\n var len = state.objectMode ? 1 : chunk.length;\n\n doWrite(stream, state, false, len, chunk, encoding, cb);\n entry = entry.next;\n state.bufferedRequestCount--;\n // if we didn't call the onwrite immediately, then\n // it means that we need to wait until it does.\n // also, that means that the chunk and cb are currently\n // being processed, so move the buffer counter past them.\n if (state.writing) {\n break;\n }\n }\n\n if (entry === null) state.lastBufferedRequest = null;\n }\n\n state.bufferedRequest = entry;\n state.bufferProcessing = false;\n}\n\nWritable.prototype._write = function (chunk, encoding, cb) {\n cb(new Error('_write() is not implemented'));\n};\n\nWritable.prototype._writev = null;\n\nWritable.prototype.end = function (chunk, encoding, cb) {\n var state = this._writableState;\n\n if (typeof chunk === 'function') {\n cb = chunk;\n chunk = null;\n encoding = null;\n } else if (typeof encoding === 'function') {\n cb = encoding;\n encoding = null;\n }\n\n if (chunk !== null && chunk !== undefined) this.write(chunk, encoding);\n\n // .end() fully uncorks\n if (state.corked) {\n state.corked = 1;\n this.uncork();\n }\n\n // ignore unnecessary end() calls.\n if (!state.ending && !state.finished) endWritable(this, state, cb);\n};\n\nfunction needFinish(state) {\n return state.ending && state.length === 0 && state.bufferedRequest === null && !state.finished && !state.writing;\n}\nfunction callFinal(stream, state) {\n stream._final(function (err) {\n state.pendingcb--;\n if (err) {\n stream.emit('error', err);\n }\n state.prefinished = true;\n stream.emit('prefinish');\n finishMaybe(stream, state);\n });\n}\nfunction prefinish(stream, state) {\n if (!state.prefinished && !state.finalCalled) {\n if (typeof stream._final === 'function') {\n state.pendingcb++;\n state.finalCalled = true;\n pna.nextTick(callFinal, stream, state);\n } else {\n state.prefinished = true;\n stream.emit('prefinish');\n }\n }\n}\n\nfunction finishMaybe(stream, state) {\n var need = needFinish(state);\n if (need) {\n prefinish(stream, state);\n if (state.pendingcb === 0) {\n state.finished = true;\n stream.emit('finish');\n }\n }\n return need;\n}\n\nfunction endWritable(stream, state, cb) {\n state.ending = true;\n finishMaybe(stream, state);\n if (cb) {\n if (state.finished) pna.nextTick(cb);else stream.once('finish', cb);\n }\n state.ended = true;\n stream.writable = false;\n}\n\nfunction onCorkedFinish(corkReq, state, err) {\n var entry = corkReq.entry;\n corkReq.entry = null;\n while (entry) {\n var cb = entry.callback;\n state.pendingcb--;\n cb(err);\n entry = entry.next;\n }\n if (state.corkedRequestsFree) {\n state.corkedRequestsFree.next = corkReq;\n } else {\n state.corkedRequestsFree = corkReq;\n }\n}\n\nObject.defineProperty(Writable.prototype, 'destroyed', {\n get: function () {\n if (this._writableState === undefined) {\n return false;\n }\n return this._writableState.destroyed;\n },\n set: function (value) {\n // we ignore the value if the stream\n // has not been initialized yet\n if (!this._writableState) {\n return;\n }\n\n // backward compatibility, the user is explicitly\n // managing destroyed\n this._writableState.destroyed = value;\n }\n});\n\nWritable.prototype.destroy = destroyImpl.destroy;\nWritable.prototype._undestroy = destroyImpl.undestroy;\nWritable.prototype._destroy = function (err, cb) {\n this.end();\n cb(err);\n};\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/_stream_writable.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/internal/streams/BufferList.js": -/*!*************************************************************************!*\ - !*** ./node_modules/readable-stream/lib/internal/streams/BufferList.js ***! - \*************************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("\n\nfunction _classCallCheck(instance, Constructor) { if (!(instance instanceof Constructor)) { throw new TypeError(\"Cannot call a class as a function\"); } }\n\nvar Buffer = __webpack_require__(/*! safe-buffer */ \"./node_modules/safe-buffer/index.js\").Buffer;\nvar util = __webpack_require__(/*! util */ \"util\");\n\nfunction copyBuffer(src, target, offset) {\n src.copy(target, offset);\n}\n\nmodule.exports = function () {\n function BufferList() {\n _classCallCheck(this, BufferList);\n\n this.head = null;\n this.tail = null;\n this.length = 0;\n }\n\n BufferList.prototype.push = function push(v) {\n var entry = { data: v, next: null };\n if (this.length > 0) this.tail.next = entry;else this.head = entry;\n this.tail = entry;\n ++this.length;\n };\n\n BufferList.prototype.unshift = function unshift(v) {\n var entry = { data: v, next: this.head };\n if (this.length === 0) this.tail = entry;\n this.head = entry;\n ++this.length;\n };\n\n BufferList.prototype.shift = function shift() {\n if (this.length === 0) return;\n var ret = this.head.data;\n if (this.length === 1) this.head = this.tail = null;else this.head = this.head.next;\n --this.length;\n return ret;\n };\n\n BufferList.prototype.clear = function clear() {\n this.head = this.tail = null;\n this.length = 0;\n };\n\n BufferList.prototype.join = function join(s) {\n if (this.length === 0) return '';\n var p = this.head;\n var ret = '' + p.data;\n while (p = p.next) {\n ret += s + p.data;\n }return ret;\n };\n\n BufferList.prototype.concat = function concat(n) {\n if (this.length === 0) return Buffer.alloc(0);\n if (this.length === 1) return this.head.data;\n var ret = Buffer.allocUnsafe(n >>> 0);\n var p = this.head;\n var i = 0;\n while (p) {\n copyBuffer(p.data, ret, i);\n i += p.data.length;\n p = p.next;\n }\n return ret;\n };\n\n return BufferList;\n}();\n\nif (util && util.inspect && util.inspect.custom) {\n module.exports.prototype[util.inspect.custom] = function () {\n var obj = util.inspect({ length: this.length });\n return this.constructor.name + ' ' + obj;\n };\n}\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/internal/streams/BufferList.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/internal/streams/destroy.js": -/*!**********************************************************************!*\ - !*** ./node_modules/readable-stream/lib/internal/streams/destroy.js ***! - \**********************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -"use strict"; -eval("\n\n/**/\n\nvar pna = __webpack_require__(/*! process-nextick-args */ \"./node_modules/process-nextick-args/index.js\");\n/**/\n\n// undocumented cb() API, needed for core, not for public API\nfunction destroy(err, cb) {\n var _this = this;\n\n var readableDestroyed = this._readableState && this._readableState.destroyed;\n var writableDestroyed = this._writableState && this._writableState.destroyed;\n\n if (readableDestroyed || writableDestroyed) {\n if (cb) {\n cb(err);\n } else if (err && (!this._writableState || !this._writableState.errorEmitted)) {\n pna.nextTick(emitErrorNT, this, err);\n }\n return this;\n }\n\n // we set destroyed to true before firing error callbacks in order\n // to make it re-entrance safe in case destroy() is called within callbacks\n\n if (this._readableState) {\n this._readableState.destroyed = true;\n }\n\n // if this is a duplex stream mark the writable part as destroyed as well\n if (this._writableState) {\n this._writableState.destroyed = true;\n }\n\n this._destroy(err || null, function (err) {\n if (!cb && err) {\n pna.nextTick(emitErrorNT, _this, err);\n if (_this._writableState) {\n _this._writableState.errorEmitted = true;\n }\n } else if (cb) {\n cb(err);\n }\n });\n\n return this;\n}\n\nfunction undestroy() {\n if (this._readableState) {\n this._readableState.destroyed = false;\n this._readableState.reading = false;\n this._readableState.ended = false;\n this._readableState.endEmitted = false;\n }\n\n if (this._writableState) {\n this._writableState.destroyed = false;\n this._writableState.ended = false;\n this._writableState.ending = false;\n this._writableState.finished = false;\n this._writableState.errorEmitted = false;\n }\n}\n\nfunction emitErrorNT(self, err) {\n self.emit('error', err);\n}\n\nmodule.exports = {\n destroy: destroy,\n undestroy: undestroy\n};\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/internal/streams/destroy.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/lib/internal/streams/stream.js": -/*!*********************************************************************!*\ - !*** ./node_modules/readable-stream/lib/internal/streams/stream.js ***! - \*********************************************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -eval("module.exports = __webpack_require__(/*! stream */ \"stream\");\n\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/lib/internal/streams/stream.js?"); - -/***/ }), - -/***/ "./node_modules/readable-stream/readable.js": -/*!**************************************************!*\ - !*** ./node_modules/readable-stream/readable.js ***! - \**************************************************/ -/***/ ((module, exports, __webpack_require__) => { - -eval("var Stream = __webpack_require__(/*! stream */ \"stream\");\nif (process.env.READABLE_STREAM === 'disable' && Stream) {\n module.exports = Stream;\n exports = module.exports = Stream.Readable;\n exports.Readable = Stream.Readable;\n exports.Writable = Stream.Writable;\n exports.Duplex = Stream.Duplex;\n exports.Transform = Stream.Transform;\n exports.PassThrough = Stream.PassThrough;\n exports.Stream = Stream;\n} else {\n exports = module.exports = __webpack_require__(/*! ./lib/_stream_readable.js */ \"./node_modules/readable-stream/lib/_stream_readable.js\");\n exports.Stream = Stream || exports;\n exports.Readable = exports;\n exports.Writable = __webpack_require__(/*! ./lib/_stream_writable.js */ \"./node_modules/readable-stream/lib/_stream_writable.js\");\n exports.Duplex = __webpack_require__(/*! ./lib/_stream_duplex.js */ \"./node_modules/readable-stream/lib/_stream_duplex.js\");\n exports.Transform = __webpack_require__(/*! ./lib/_stream_transform.js */ \"./node_modules/readable-stream/lib/_stream_transform.js\");\n exports.PassThrough = __webpack_require__(/*! ./lib/_stream_passthrough.js */ \"./node_modules/readable-stream/lib/_stream_passthrough.js\");\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/readable-stream/readable.js?"); - -/***/ }), - -/***/ "./node_modules/safe-buffer/index.js": -/*!*******************************************!*\ - !*** ./node_modules/safe-buffer/index.js ***! - \*******************************************/ -/***/ ((module, exports, __webpack_require__) => { - -eval("/* eslint-disable node/no-deprecated-api */\nvar buffer = __webpack_require__(/*! buffer */ \"buffer\")\nvar Buffer = buffer.Buffer\n\n// alternative to using Object.keys for old browsers\nfunction copyProps (src, dst) {\n for (var key in src) {\n dst[key] = src[key]\n }\n}\nif (Buffer.from && Buffer.alloc && Buffer.allocUnsafe && Buffer.allocUnsafeSlow) {\n module.exports = buffer\n} else {\n // Copy properties from require('buffer')\n copyProps(buffer, exports)\n exports.Buffer = SafeBuffer\n}\n\nfunction SafeBuffer (arg, encodingOrOffset, length) {\n return Buffer(arg, encodingOrOffset, length)\n}\n\n// Copy static methods from Buffer\ncopyProps(Buffer, SafeBuffer)\n\nSafeBuffer.from = function (arg, encodingOrOffset, length) {\n if (typeof arg === 'number') {\n throw new TypeError('Argument must not be a number')\n }\n return Buffer(arg, encodingOrOffset, length)\n}\n\nSafeBuffer.alloc = function (size, fill, encoding) {\n if (typeof size !== 'number') {\n throw new TypeError('Argument must be a number')\n }\n var buf = Buffer(size)\n if (fill !== undefined) {\n if (typeof encoding === 'string') {\n buf.fill(fill, encoding)\n } else {\n buf.fill(fill)\n }\n } else {\n buf.fill(0)\n }\n return buf\n}\n\nSafeBuffer.allocUnsafe = function (size) {\n if (typeof size !== 'number') {\n throw new TypeError('Argument must be a number')\n }\n return Buffer(size)\n}\n\nSafeBuffer.allocUnsafeSlow = function (size) {\n if (typeof size !== 'number') {\n throw new TypeError('Argument must be a number')\n }\n return buffer.SlowBuffer(size)\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/safe-buffer/index.js?"); - -/***/ }), - -/***/ "./node_modules/shell-source/index.js": -/*!********************************************!*\ - !*** ./node_modules/shell-source/index.js ***! - \********************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -eval("var spawn = __webpack_require__(/*! child_process */ \"child_process\").spawn;\nvar concat = __webpack_require__(/*! concat-stream */ \"./node_modules/concat-stream/index.js\");\nvar xtend = __webpack_require__(/*! xtend */ \"./node_modules/xtend/immutable.js\");\n\nvar reserved = {\n 'SHLVL': true,\n '_': true,\n};\n\nmodule.exports = function(filename, opts, cb) {\n if (typeof opts === 'function') {\n cb = opts;\n opts = {};\n }\n\n if (opts.source === undefined) {\n opts.source = true;\n }\n\n if (!opts.wrapper) {\n opts.wrapper = __dirname + '/source.sh';\n }\n\n if (!opts.reserved) {\n opts.reserved = xtend(reserved, opts.reserved);\n }\n else {\n opts.reserved = xtend(reserved);\n }\n\n var env = {};\n var count = 0;\n var status = null;\n var stdout = null;\n var stderr = null;\n var proc = spawn(opts.wrapper, [ filename ]);\n \n proc.on('error', function(err) {\n cb(err);\n cb = null;\n });\n\n proc.on('exit', function(code) {\n status = code;\n ++count === 3 && done();\n });\n\n proc.stdout.pipe(concat(function(data) {\n stdout = data;\n ++count === 3 && done();\n }));\n\n proc.stderr.pipe(concat(function(data) {\n stderr = data;\n ++count === 3 && done();\n }));\n\n function done() {\n if (!cb) return;\n\n if (status === 0) {\n var envstring = stdout.toString().split('\\n');\n\n for (var i=0; i { - -"use strict"; -eval("// Copyright Joyent, Inc. and other Node contributors.\n//\n// Permission is hereby granted, free of charge, to any person obtaining a\n// copy of this software and associated documentation files (the\n// \"Software\"), to deal in the Software without restriction, including\n// without limitation the rights to use, copy, modify, merge, publish,\n// distribute, sublicense, and/or sell copies of the Software, and to permit\n// persons to whom the Software is furnished to do so, subject to the\n// following conditions:\n//\n// The above copyright notice and this permission notice shall be included\n// in all copies or substantial portions of the Software.\n//\n// THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS\n// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF\n// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN\n// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,\n// DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR\n// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE\n// USE OR OTHER DEALINGS IN THE SOFTWARE.\n\n\n\n/**/\n\nvar Buffer = __webpack_require__(/*! safe-buffer */ \"./node_modules/safe-buffer/index.js\").Buffer;\n/**/\n\nvar isEncoding = Buffer.isEncoding || function (encoding) {\n encoding = '' + encoding;\n switch (encoding && encoding.toLowerCase()) {\n case 'hex':case 'utf8':case 'utf-8':case 'ascii':case 'binary':case 'base64':case 'ucs2':case 'ucs-2':case 'utf16le':case 'utf-16le':case 'raw':\n return true;\n default:\n return false;\n }\n};\n\nfunction _normalizeEncoding(enc) {\n if (!enc) return 'utf8';\n var retried;\n while (true) {\n switch (enc) {\n case 'utf8':\n case 'utf-8':\n return 'utf8';\n case 'ucs2':\n case 'ucs-2':\n case 'utf16le':\n case 'utf-16le':\n return 'utf16le';\n case 'latin1':\n case 'binary':\n return 'latin1';\n case 'base64':\n case 'ascii':\n case 'hex':\n return enc;\n default:\n if (retried) return; // undefined\n enc = ('' + enc).toLowerCase();\n retried = true;\n }\n }\n};\n\n// Do not cache `Buffer.isEncoding` when checking encoding names as some\n// modules monkey-patch it to support additional encodings\nfunction normalizeEncoding(enc) {\n var nenc = _normalizeEncoding(enc);\n if (typeof nenc !== 'string' && (Buffer.isEncoding === isEncoding || !isEncoding(enc))) throw new Error('Unknown encoding: ' + enc);\n return nenc || enc;\n}\n\n// StringDecoder provides an interface for efficiently splitting a series of\n// buffers into a series of JS strings without breaking apart multi-byte\n// characters.\nexports.StringDecoder = StringDecoder;\nfunction StringDecoder(encoding) {\n this.encoding = normalizeEncoding(encoding);\n var nb;\n switch (this.encoding) {\n case 'utf16le':\n this.text = utf16Text;\n this.end = utf16End;\n nb = 4;\n break;\n case 'utf8':\n this.fillLast = utf8FillLast;\n nb = 4;\n break;\n case 'base64':\n this.text = base64Text;\n this.end = base64End;\n nb = 3;\n break;\n default:\n this.write = simpleWrite;\n this.end = simpleEnd;\n return;\n }\n this.lastNeed = 0;\n this.lastTotal = 0;\n this.lastChar = Buffer.allocUnsafe(nb);\n}\n\nStringDecoder.prototype.write = function (buf) {\n if (buf.length === 0) return '';\n var r;\n var i;\n if (this.lastNeed) {\n r = this.fillLast(buf);\n if (r === undefined) return '';\n i = this.lastNeed;\n this.lastNeed = 0;\n } else {\n i = 0;\n }\n if (i < buf.length) return r ? r + this.text(buf, i) : this.text(buf, i);\n return r || '';\n};\n\nStringDecoder.prototype.end = utf8End;\n\n// Returns only complete characters in a Buffer\nStringDecoder.prototype.text = utf8Text;\n\n// Attempts to complete a partial non-UTF-8 character using bytes from a Buffer\nStringDecoder.prototype.fillLast = function (buf) {\n if (this.lastNeed <= buf.length) {\n buf.copy(this.lastChar, this.lastTotal - this.lastNeed, 0, this.lastNeed);\n return this.lastChar.toString(this.encoding, 0, this.lastTotal);\n }\n buf.copy(this.lastChar, this.lastTotal - this.lastNeed, 0, buf.length);\n this.lastNeed -= buf.length;\n};\n\n// Checks the type of a UTF-8 byte, whether it's ASCII, a leading byte, or a\n// continuation byte. If an invalid byte is detected, -2 is returned.\nfunction utf8CheckByte(byte) {\n if (byte <= 0x7F) return 0;else if (byte >> 5 === 0x06) return 2;else if (byte >> 4 === 0x0E) return 3;else if (byte >> 3 === 0x1E) return 4;\n return byte >> 6 === 0x02 ? -1 : -2;\n}\n\n// Checks at most 3 bytes at the end of a Buffer in order to detect an\n// incomplete multi-byte UTF-8 character. The total number of bytes (2, 3, or 4)\n// needed to complete the UTF-8 character (if applicable) are returned.\nfunction utf8CheckIncomplete(self, buf, i) {\n var j = buf.length - 1;\n if (j < i) return 0;\n var nb = utf8CheckByte(buf[j]);\n if (nb >= 0) {\n if (nb > 0) self.lastNeed = nb - 1;\n return nb;\n }\n if (--j < i || nb === -2) return 0;\n nb = utf8CheckByte(buf[j]);\n if (nb >= 0) {\n if (nb > 0) self.lastNeed = nb - 2;\n return nb;\n }\n if (--j < i || nb === -2) return 0;\n nb = utf8CheckByte(buf[j]);\n if (nb >= 0) {\n if (nb > 0) {\n if (nb === 2) nb = 0;else self.lastNeed = nb - 3;\n }\n return nb;\n }\n return 0;\n}\n\n// Validates as many continuation bytes for a multi-byte UTF-8 character as\n// needed or are available. If we see a non-continuation byte where we expect\n// one, we \"replace\" the validated continuation bytes we've seen so far with\n// a single UTF-8 replacement character ('\\ufffd'), to match v8's UTF-8 decoding\n// behavior. The continuation byte check is included three times in the case\n// where all of the continuation bytes for a character exist in the same buffer.\n// It is also done this way as a slight performance increase instead of using a\n// loop.\nfunction utf8CheckExtraBytes(self, buf, p) {\n if ((buf[0] & 0xC0) !== 0x80) {\n self.lastNeed = 0;\n return '\\ufffd';\n }\n if (self.lastNeed > 1 && buf.length > 1) {\n if ((buf[1] & 0xC0) !== 0x80) {\n self.lastNeed = 1;\n return '\\ufffd';\n }\n if (self.lastNeed > 2 && buf.length > 2) {\n if ((buf[2] & 0xC0) !== 0x80) {\n self.lastNeed = 2;\n return '\\ufffd';\n }\n }\n }\n}\n\n// Attempts to complete a multi-byte UTF-8 character using bytes from a Buffer.\nfunction utf8FillLast(buf) {\n var p = this.lastTotal - this.lastNeed;\n var r = utf8CheckExtraBytes(this, buf, p);\n if (r !== undefined) return r;\n if (this.lastNeed <= buf.length) {\n buf.copy(this.lastChar, p, 0, this.lastNeed);\n return this.lastChar.toString(this.encoding, 0, this.lastTotal);\n }\n buf.copy(this.lastChar, p, 0, buf.length);\n this.lastNeed -= buf.length;\n}\n\n// Returns all complete UTF-8 characters in a Buffer. If the Buffer ended on a\n// partial character, the character's bytes are buffered until the required\n// number of bytes are available.\nfunction utf8Text(buf, i) {\n var total = utf8CheckIncomplete(this, buf, i);\n if (!this.lastNeed) return buf.toString('utf8', i);\n this.lastTotal = total;\n var end = buf.length - (total - this.lastNeed);\n buf.copy(this.lastChar, 0, end);\n return buf.toString('utf8', i, end);\n}\n\n// For UTF-8, a replacement character is added when ending on a partial\n// character.\nfunction utf8End(buf) {\n var r = buf && buf.length ? this.write(buf) : '';\n if (this.lastNeed) return r + '\\ufffd';\n return r;\n}\n\n// UTF-16LE typically needs two bytes per character, but even if we have an even\n// number of bytes available, we need to check if we end on a leading/high\n// surrogate. In that case, we need to wait for the next two bytes in order to\n// decode the last character properly.\nfunction utf16Text(buf, i) {\n if ((buf.length - i) % 2 === 0) {\n var r = buf.toString('utf16le', i);\n if (r) {\n var c = r.charCodeAt(r.length - 1);\n if (c >= 0xD800 && c <= 0xDBFF) {\n this.lastNeed = 2;\n this.lastTotal = 4;\n this.lastChar[0] = buf[buf.length - 2];\n this.lastChar[1] = buf[buf.length - 1];\n return r.slice(0, -1);\n }\n }\n return r;\n }\n this.lastNeed = 1;\n this.lastTotal = 2;\n this.lastChar[0] = buf[buf.length - 1];\n return buf.toString('utf16le', i, buf.length - 1);\n}\n\n// For UTF-16LE we do not explicitly append special replacement characters if we\n// end on a partial character, we simply let v8 handle that.\nfunction utf16End(buf) {\n var r = buf && buf.length ? this.write(buf) : '';\n if (this.lastNeed) {\n var end = this.lastTotal - this.lastNeed;\n return r + this.lastChar.toString('utf16le', 0, end);\n }\n return r;\n}\n\nfunction base64Text(buf, i) {\n var n = (buf.length - i) % 3;\n if (n === 0) return buf.toString('base64', i);\n this.lastNeed = 3 - n;\n this.lastTotal = 3;\n if (n === 1) {\n this.lastChar[0] = buf[buf.length - 1];\n } else {\n this.lastChar[0] = buf[buf.length - 2];\n this.lastChar[1] = buf[buf.length - 1];\n }\n return buf.toString('base64', i, buf.length - n);\n}\n\nfunction base64End(buf) {\n var r = buf && buf.length ? this.write(buf) : '';\n if (this.lastNeed) return r + this.lastChar.toString('base64', 0, 3 - this.lastNeed);\n return r;\n}\n\n// Pass bytes on through for single-byte encodings (e.g. ascii, latin1, hex)\nfunction simpleWrite(buf) {\n return buf.toString(this.encoding);\n}\n\nfunction simpleEnd(buf) {\n return buf && buf.length ? this.write(buf) : '';\n}\n\n//# sourceURL=webpack://electron-test/./node_modules/string_decoder/lib/string_decoder.js?"); - -/***/ }), - -/***/ "./electron/gamepad.ts": -/*!*****************************!*\ - !*** ./electron/gamepad.ts ***! - \*****************************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nexports.gamepadListener = void 0;\nvar path_1 = __importDefault(__webpack_require__(/*! path */ \"path\"));\nvar child_process_1 = __webpack_require__(/*! child_process */ \"child_process\");\nvar LogItem_1 = __importDefault(__webpack_require__(/*! ../src/components/Log/LogItem */ \"./src/components/Log/LogItem.ts\"));\nvar channels_1 = __webpack_require__(/*! ../src/components/Log/channels */ \"./src/components/Log/channels.ts\");\nvar timeout = 0;\nfunction gamepadListener(win) {\n return __awaiter(this, void 0, void 0, function () {\n var sender;\n return __generator(this, function (_a) {\n sender = child_process_1.spawn('python3', ['-u', path_1[\"default\"].resolve(__dirname, '../ros/src/gamepad/src/sender.py')]);\n timeout = Date.now();\n sender.on('exit', function (code) {\n if (Date.now() - timeout > 1000) {\n win.webContents.send(channels_1.GAMEPAD, LogItem_1[\"default\"]('gamepad_listener', 'Error! Gamepad lost. Reacquiring...'));\n }\n setTimeout(function () {\n gamepadListener(win);\n }, 1000);\n });\n sender.stderr.on('data', function (data) {\n console.log(\"Gamepad Listener:\\n\" + data);\n win.webContents.send(channels_1.GAMEPAD, LogItem_1[\"default\"]('gamepad_listener', \"Error: \" + data));\n });\n sender.stdout.on('data', function (data) {\n console.log(data.toString());\n if ((\"\" + data).includes('ready')) {\n win.webContents.send(channels_1.GAMEPAD, LogItem_1[\"default\"]('gamepad_listener', 'Gamepad connected'));\n }\n });\n win.on('close', function (_) {\n sender.kill('SIGINT');\n sender.kill('SIGINT');\n });\n return [2 /*return*/];\n });\n });\n}\nexports.gamepadListener = gamepadListener;\n\n\n//# sourceURL=webpack://electron-test/./electron/gamepad.ts?"); - -/***/ }), - -/***/ "./electron/imu.ts": -/*!*************************!*\ - !*** ./electron/imu.ts ***! - \*************************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nvar path_1 = __importDefault(__webpack_require__(/*! path */ \"path\"));\nvar child_process_1 = __webpack_require__(/*! child_process */ \"child_process\");\nvar channels_1 = __webpack_require__(/*! ../src/components/Log/channels */ \"./src/components/Log/channels.ts\");\nvar LogItem_1 = __importDefault(__webpack_require__(/*! ../src/components/Log/LogItem */ \"./src/components/Log/LogItem.ts\"));\nfunction imu(win) {\n return __awaiter(this, void 0, void 0, function () {\n var listener;\n return __generator(this, function (_a) {\n listener = child_process_1.spawn('python3', ['-u', path_1[\"default\"].resolve(__dirname, '../ros/src/imu/src/status.py')]);\n win.webContents.send(channels_1.IMU, LogItem_1[\"default\"]('imu', 'Started node'));\n listener.on('exit', function (code) {\n win.webContents.send(channels_1.IMU, LogItem_1[\"default\"]('imu', 'Node exited'));\n });\n listener.stdout.on('data', function (data) {\n try {\n var arr = JSON.parse(data.toString().split('\\n')[0]);\n win.webContents.send('imu', arr);\n }\n catch (e) {\n console.log(e);\n }\n });\n listener.stderr.on('data', function (data) {\n win.webContents.send(channels_1.IMU, LogItem_1[\"default\"]('imu', \"Error: \" + data));\n });\n win.on('close', function (_) {\n listener.kill('SIGINT');\n });\n return [2 /*return*/];\n });\n });\n}\nexports.default = imu;\n\n\n//# sourceURL=webpack://electron-test/./electron/imu.ts?"); - -/***/ }), - -/***/ "./electron/servo.ts": -/*!***************************!*\ - !*** ./electron/servo.ts ***! - \***************************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nvar path_1 = __importDefault(__webpack_require__(/*! path */ \"path\"));\nvar child_process_1 = __webpack_require__(/*! child_process */ \"child_process\");\nvar LogItem_1 = __importDefault(__webpack_require__(/*! ../src/components/Log/LogItem */ \"./src/components/Log/LogItem.ts\"));\nvar channels_1 = __webpack_require__(/*! ../src/components/Log/channels */ \"./src/components/Log/channels.ts\");\nvar net_1 = __importDefault(__webpack_require__(/*! net */ \"net\"));\nvar electron_1 = __webpack_require__(/*! electron */ \"electron\");\nfunction send(win) {\n var sock = net_1[\"default\"].connect(11002, undefined, function () {\n win.webContents.send(channels_1.SERVO, LogItem_1[\"default\"]('servo', 'Socket connected'));\n });\n electron_1.ipcMain.on('servo_send', function (e, data) {\n sock.write(\"\" + data);\n });\n sock.on('error', function (_) { return win.webContents.send(channels_1.SERVO, LogItem_1[\"default\"]('servo', 'Socket error!')); });\n win.on('close', function (_) {\n sock.end();\n });\n}\nfunction servo(win) {\n return __awaiter(this, void 0, void 0, function () {\n var sender;\n return __generator(this, function (_a) {\n sender = child_process_1.spawn('python3', ['-u', path_1[\"default\"].resolve(__dirname, '../ros/src/cam_servo/src/servo_sender.py'), '11002']);\n win.webContents.send(channels_1.SERVO, LogItem_1[\"default\"]('servo', 'Started'));\n sender.on('exit', function (code) {\n win.webContents.send(channels_1.SERVO, LogItem_1[\"default\"]('servo', 'Exiting...'));\n });\n sender.stdout.on('data', function (data) {\n if ((\"\" + data).includes('ready')) {\n send(win);\n }\n });\n sender.stderr.on('data', function (data) {\n console.log(\"Gamepad Listener:\\n\" + data);\n win.webContents.send(channels_1.SERVO, LogItem_1[\"default\"]('servo', \"Error: \" + data));\n });\n win.on('close', function (_) {\n sender.kill('SIGINT');\n });\n return [2 /*return*/];\n });\n });\n}\nexports.default = servo;\n\n\n//# sourceURL=webpack://electron-test/./electron/servo.ts?"); - -/***/ }), - -/***/ "./electron/setIp.ts": -/*!***************************!*\ - !*** ./electron/setIp.ts ***! - \***************************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nvar os_1 = __importDefault(__webpack_require__(/*! os */ \"os\"));\nfunction getIp() {\n return __awaiter(this, void 0, void 0, function () {\n var inter, i, dev;\n return __generator(this, function (_a) {\n inter = os_1[\"default\"].networkInterfaces();\n for (dev in inter) {\n i = inter[dev].filter(function (details) {\n return details.family == 'IPv4' && !details.internal;\n });\n if (i.length > 0)\n return [2 /*return*/, i[0].address];\n }\n return [2 /*return*/, 'localhost'];\n });\n });\n}\nfunction setIp() {\n return __awaiter(this, void 0, void 0, function () {\n var ip;\n return __generator(this, function (_a) {\n switch (_a.label) {\n case 0: return [4 /*yield*/, getIp()];\n case 1:\n ip = _a.sent();\n process.env.ROS_HOSTNAME = ip;\n process.env.ROS_IP = ip;\n process.env.ROS_MASTER_URI = 'http://192.168.1.3:11311';\n return [2 /*return*/, ip];\n }\n });\n });\n}\nexports.default = setIp;\n\n\n//# sourceURL=webpack://electron-test/./electron/setIp.ts?"); - -/***/ }), - -/***/ "./electron/setupRos.ts": -/*!******************************!*\ - !*** ./electron/setupRos.ts ***! - \******************************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nvar path_1 = __importDefault(__webpack_require__(/*! path */ \"path\"));\nvar child_process_1 = __webpack_require__(/*! child_process */ \"child_process\");\nvar shell_source_1 = __importDefault(__webpack_require__(/*! shell-source */ \"./node_modules/shell-source/index.js\"));\nvar sourceRos = function () { return __awaiter(void 0, void 0, void 0, function () {\n return __generator(this, function (_a) {\n return [2 /*return*/, new Promise(function (resolve, reject) {\n shell_source_1[\"default\"](path_1[\"default\"].resolve('../ros/devel/setup.bash'), function (err) {\n if (err)\n reject(process.env);\n resolve(process.env);\n });\n })];\n });\n}); };\nfunction setupRos() {\n return __awaiter(this, void 0, void 0, function () {\n var stdout, env;\n return __generator(this, function (_a) {\n switch (_a.label) {\n case 0:\n process.chdir('./ros');\n stdout = child_process_1.execSync('catkin_make');\n return [4 /*yield*/, sourceRos()];\n case 1:\n env = _a.sent();\n process.chdir('../');\n return [2 /*return*/, env];\n }\n });\n });\n}\nexports.default = setupRos;\n\n\n//# sourceURL=webpack://electron-test/./electron/setupRos.ts?"); - -/***/ }), - -/***/ "./electron/thrusters.ts": -/*!*******************************!*\ - !*** ./electron/thrusters.ts ***! - \*******************************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nvar path_1 = __importDefault(__webpack_require__(/*! path */ \"path\"));\nvar child_process_1 = __webpack_require__(/*! child_process */ \"child_process\");\nvar channels_1 = __webpack_require__(/*! ../src/components/Log/channels */ \"./src/components/Log/channels.ts\");\nvar LogItem_1 = __importDefault(__webpack_require__(/*! ../src/components/Log/LogItem */ \"./src/components/Log/LogItem.ts\"));\nfunction thrusters(win) {\n return __awaiter(this, void 0, void 0, function () {\n var listener;\n return __generator(this, function (_a) {\n listener = child_process_1.spawn('python3', ['-u', path_1[\"default\"].resolve(__dirname, '../ros/src/thrusters/src/status.py')]);\n win.webContents.send(channels_1.THRUSTERS, LogItem_1[\"default\"]('thrusters', 'Started node'));\n listener.on('exit', function (code) {\n win.webContents.send(channels_1.THRUSTERS, LogItem_1[\"default\"]('thrusters', 'Node exited'));\n });\n listener.stdout.on('data', function (data) {\n try {\n var arr = JSON.parse(data.toString().split('\\n')[0]);\n win.webContents.send('thrusters', arr);\n }\n catch (e) {\n }\n });\n listener.stderr.on('data', function (data) {\n win.webContents.send(channels_1.THRUSTERS, LogItem_1[\"default\"]('thrusters', \"Error: \" + data));\n });\n win.on('close', function (_) {\n listener.kill('SIGINT');\n });\n return [2 /*return*/];\n });\n });\n}\nexports.default = thrusters;\n\n\n//# sourceURL=webpack://electron-test/./electron/thrusters.ts?"); - -/***/ }), - -/***/ "./main.ts": -/*!*****************!*\ - !*** ./main.ts ***! - \*****************/ -/***/ (function(__unused_webpack_module, exports, __webpack_require__) { - -"use strict"; -eval("\nvar __awaiter = (this && this.__awaiter) || function (thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n};\nvar __generator = (this && this.__generator) || function (thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g;\n return g = { next: verb(0), \"throw\": verb(1), \"return\": verb(2) }, typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (_) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n};\nvar __importDefault = (this && this.__importDefault) || function (mod) {\n return (mod && mod.__esModule) ? mod : { \"default\": mod };\n};\nexports.__esModule = true;\nvar electron_1 = __webpack_require__(/*! electron */ \"electron\");\nvar setIp_1 = __importDefault(__webpack_require__(/*! ./electron/setIp */ \"./electron/setIp.ts\"));\nvar setupRos_1 = __importDefault(__webpack_require__(/*! ./electron/setupRos */ \"./electron/setupRos.ts\"));\nvar gamepad_1 = __webpack_require__(/*! ./electron/gamepad */ \"./electron/gamepad.ts\");\nvar LogItem_1 = __importDefault(__webpack_require__(/*! ./src/components/Log/LogItem */ \"./src/components/Log/LogItem.ts\"));\nvar channels_1 = __webpack_require__(/*! ./src/components/Log/channels */ \"./src/components/Log/channels.ts\");\nvar servo_1 = __importDefault(__webpack_require__(/*! ./electron/servo */ \"./electron/servo.ts\"));\nvar thrusters_1 = __importDefault(__webpack_require__(/*! ./electron/thrusters */ \"./electron/thrusters.ts\"));\nvar imu_1 = __importDefault(__webpack_require__(/*! ./electron/imu */ \"./electron/imu.ts\"));\nvar nodeManager = function (win) { return __awaiter(void 0, void 0, void 0, function () {\n return __generator(this, function (_a) {\n switch (_a.label) {\n case 0: return [4 /*yield*/, setIp_1[\"default\"]().then(function (addr) {\n win.webContents.send(channels_1.SET_IP, LogItem_1[\"default\"]('ip_set', \"Found \" + addr + \" as local machine\"));\n })];\n case 1:\n _a.sent();\n setupRos_1[\"default\"]().then(function (env) {\n process.env = env;\n gamepad_1.gamepadListener(win);\n win.webContents.send(channels_1.CATKIN_MAKE, LogItem_1[\"default\"]('catkin_make', 'Built and sourced'));\n servo_1[\"default\"](win)[\"catch\"](function (e) {\n console.log(e);\n });\n thrusters_1[\"default\"](win)[\"catch\"](function (e) {\n console.log(e);\n });\n imu_1[\"default\"](win)[\"catch\"](function (e) {\n console.log(e);\n });\n })[\"catch\"](function (err) {\n win.webContents.send(channels_1.CATKIN_MAKE, LogItem_1[\"default\"]('catkin_make', \"Error: \" + err));\n });\n return [2 /*return*/];\n }\n });\n}); };\nfunction createWindow() {\n // Create the browser window.\n var win = new electron_1.BrowserWindow({\n width: 1920,\n height: 1080,\n webPreferences: {\n nodeIntegration: true,\n webSecurity: false,\n contextIsolation: false,\n }\n });\n // and load the index.html of the app.\n win.loadFile('./index.html');\n //win.removeMenu();\n electron_1.ipcMain.on('logger', function (e, msg) {\n if (msg == 'ready')\n nodeManager(win);\n });\n}\nelectron_1.app.on('ready', createWindow);\n\n\n//# sourceURL=webpack://electron-test/./main.ts?"); - -/***/ }), - -/***/ "./src/components/Log/LogItem.ts": -/*!***************************************!*\ - !*** ./src/components/Log/LogItem.ts ***! - \***************************************/ -/***/ ((__unused_webpack_module, exports) => { - -"use strict"; -eval("\nexports.__esModule = true;\nfunction default_1(proc, text) {\n var i = {\n timestamp: new Date().toISOString().substr(14, 5),\n process: proc,\n text: text\n };\n return i;\n}\nexports.default = default_1;\n\n\n//# sourceURL=webpack://electron-test/./src/components/Log/LogItem.ts?"); - -/***/ }), - -/***/ "./src/components/Log/channels.ts": -/*!****************************************!*\ - !*** ./src/components/Log/channels.ts ***! - \****************************************/ -/***/ ((__unused_webpack_module, exports) => { - -"use strict"; -eval("\nexports.__esModule = true;\nexports.IMU = exports.THRUSTERS = exports.SERVO = exports.GAMEPAD = exports.LOGGER = exports.CATKIN_MAKE = exports.SET_IP = void 0;\nexports.SET_IP = 'set_ip';\nexports.CATKIN_MAKE = 'catkin_make';\nexports.LOGGER = 'logger';\nexports.GAMEPAD = 'gamepad_listener';\nexports.SERVO = 'servo';\nexports.THRUSTERS = 'thruster_listener';\nexports.IMU = 'imu_listener';\nexports.default = [\n exports.SET_IP,\n exports.CATKIN_MAKE,\n exports.LOGGER,\n exports.GAMEPAD,\n exports.SERVO,\n exports.THRUSTERS,\n exports.IMU\n];\n\n\n//# sourceURL=webpack://electron-test/./src/components/Log/channels.ts?"); - -/***/ }), - -/***/ "./node_modules/typedarray/index.js": -/*!******************************************!*\ - !*** ./node_modules/typedarray/index.js ***! - \******************************************/ -/***/ ((__unused_webpack_module, exports) => { - -eval("var undefined = (void 0); // Paranoia\n\n// Beyond this value, index getters/setters (i.e. array[0], array[1]) are so slow to\n// create, and consume so much memory, that the browser appears frozen.\nvar MAX_ARRAY_LENGTH = 1e5;\n\n// Approximations of internal ECMAScript conversion functions\nvar ECMAScript = (function() {\n // Stash a copy in case other scripts modify these\n var opts = Object.prototype.toString,\n ophop = Object.prototype.hasOwnProperty;\n\n return {\n // Class returns internal [[Class]] property, used to avoid cross-frame instanceof issues:\n Class: function(v) { return opts.call(v).replace(/^\\[object *|\\]$/g, ''); },\n HasProperty: function(o, p) { return p in o; },\n HasOwnProperty: function(o, p) { return ophop.call(o, p); },\n IsCallable: function(o) { return typeof o === 'function'; },\n ToInt32: function(v) { return v >> 0; },\n ToUint32: function(v) { return v >>> 0; }\n };\n}());\n\n// Snapshot intrinsics\nvar LN2 = Math.LN2,\n abs = Math.abs,\n floor = Math.floor,\n log = Math.log,\n min = Math.min,\n pow = Math.pow,\n round = Math.round;\n\n// ES5: lock down object properties\nfunction configureProperties(obj) {\n if (getOwnPropNames && defineProp) {\n var props = getOwnPropNames(obj), i;\n for (i = 0; i < props.length; i += 1) {\n defineProp(obj, props[i], {\n value: obj[props[i]],\n writable: false,\n enumerable: false,\n configurable: false\n });\n }\n }\n}\n\n// emulate ES5 getter/setter API using legacy APIs\n// http://blogs.msdn.com/b/ie/archive/2010/09/07/transitioning-existing-code-to-the-es5-getter-setter-apis.aspx\n// (second clause tests for Object.defineProperty() in IE<9 that only supports extending DOM prototypes, but\n// note that IE<9 does not support __defineGetter__ or __defineSetter__ so it just renders the method harmless)\nvar defineProp\nif (Object.defineProperty && (function() {\n try {\n Object.defineProperty({}, 'x', {});\n return true;\n } catch (e) {\n return false;\n }\n })()) {\n defineProp = Object.defineProperty;\n} else {\n defineProp = function(o, p, desc) {\n if (!o === Object(o)) throw new TypeError(\"Object.defineProperty called on non-object\");\n if (ECMAScript.HasProperty(desc, 'get') && Object.prototype.__defineGetter__) { Object.prototype.__defineGetter__.call(o, p, desc.get); }\n if (ECMAScript.HasProperty(desc, 'set') && Object.prototype.__defineSetter__) { Object.prototype.__defineSetter__.call(o, p, desc.set); }\n if (ECMAScript.HasProperty(desc, 'value')) { o[p] = desc.value; }\n return o;\n };\n}\n\nvar getOwnPropNames = Object.getOwnPropertyNames || function (o) {\n if (o !== Object(o)) throw new TypeError(\"Object.getOwnPropertyNames called on non-object\");\n var props = [], p;\n for (p in o) {\n if (ECMAScript.HasOwnProperty(o, p)) {\n props.push(p);\n }\n }\n return props;\n};\n\n// ES5: Make obj[index] an alias for obj._getter(index)/obj._setter(index, value)\n// for index in 0 ... obj.length\nfunction makeArrayAccessors(obj) {\n if (!defineProp) { return; }\n\n if (obj.length > MAX_ARRAY_LENGTH) throw new RangeError(\"Array too large for polyfill\");\n\n function makeArrayAccessor(index) {\n defineProp(obj, index, {\n 'get': function() { return obj._getter(index); },\n 'set': function(v) { obj._setter(index, v); },\n enumerable: true,\n configurable: false\n });\n }\n\n var i;\n for (i = 0; i < obj.length; i += 1) {\n makeArrayAccessor(i);\n }\n}\n\n// Internal conversion functions:\n// pack() - take a number (interpreted as Type), output a byte array\n// unpack() - take a byte array, output a Type-like number\n\nfunction as_signed(value, bits) { var s = 32 - bits; return (value << s) >> s; }\nfunction as_unsigned(value, bits) { var s = 32 - bits; return (value << s) >>> s; }\n\nfunction packI8(n) { return [n & 0xff]; }\nfunction unpackI8(bytes) { return as_signed(bytes[0], 8); }\n\nfunction packU8(n) { return [n & 0xff]; }\nfunction unpackU8(bytes) { return as_unsigned(bytes[0], 8); }\n\nfunction packU8Clamped(n) { n = round(Number(n)); return [n < 0 ? 0 : n > 0xff ? 0xff : n & 0xff]; }\n\nfunction packI16(n) { return [(n >> 8) & 0xff, n & 0xff]; }\nfunction unpackI16(bytes) { return as_signed(bytes[0] << 8 | bytes[1], 16); }\n\nfunction packU16(n) { return [(n >> 8) & 0xff, n & 0xff]; }\nfunction unpackU16(bytes) { return as_unsigned(bytes[0] << 8 | bytes[1], 16); }\n\nfunction packI32(n) { return [(n >> 24) & 0xff, (n >> 16) & 0xff, (n >> 8) & 0xff, n & 0xff]; }\nfunction unpackI32(bytes) { return as_signed(bytes[0] << 24 | bytes[1] << 16 | bytes[2] << 8 | bytes[3], 32); }\n\nfunction packU32(n) { return [(n >> 24) & 0xff, (n >> 16) & 0xff, (n >> 8) & 0xff, n & 0xff]; }\nfunction unpackU32(bytes) { return as_unsigned(bytes[0] << 24 | bytes[1] << 16 | bytes[2] << 8 | bytes[3], 32); }\n\nfunction packIEEE754(v, ebits, fbits) {\n\n var bias = (1 << (ebits - 1)) - 1,\n s, e, f, ln,\n i, bits, str, bytes;\n\n function roundToEven(n) {\n var w = floor(n), f = n - w;\n if (f < 0.5)\n return w;\n if (f > 0.5)\n return w + 1;\n return w % 2 ? w + 1 : w;\n }\n\n // Compute sign, exponent, fraction\n if (v !== v) {\n // NaN\n // http://dev.w3.org/2006/webapi/WebIDL/#es-type-mapping\n e = (1 << ebits) - 1; f = pow(2, fbits - 1); s = 0;\n } else if (v === Infinity || v === -Infinity) {\n e = (1 << ebits) - 1; f = 0; s = (v < 0) ? 1 : 0;\n } else if (v === 0) {\n e = 0; f = 0; s = (1 / v === -Infinity) ? 1 : 0;\n } else {\n s = v < 0;\n v = abs(v);\n\n if (v >= pow(2, 1 - bias)) {\n e = min(floor(log(v) / LN2), 1023);\n f = roundToEven(v / pow(2, e) * pow(2, fbits));\n if (f / pow(2, fbits) >= 2) {\n e = e + 1;\n f = 1;\n }\n if (e > bias) {\n // Overflow\n e = (1 << ebits) - 1;\n f = 0;\n } else {\n // Normalized\n e = e + bias;\n f = f - pow(2, fbits);\n }\n } else {\n // Denormalized\n e = 0;\n f = roundToEven(v / pow(2, 1 - bias - fbits));\n }\n }\n\n // Pack sign, exponent, fraction\n bits = [];\n for (i = fbits; i; i -= 1) { bits.push(f % 2 ? 1 : 0); f = floor(f / 2); }\n for (i = ebits; i; i -= 1) { bits.push(e % 2 ? 1 : 0); e = floor(e / 2); }\n bits.push(s ? 1 : 0);\n bits.reverse();\n str = bits.join('');\n\n // Bits to bytes\n bytes = [];\n while (str.length) {\n bytes.push(parseInt(str.substring(0, 8), 2));\n str = str.substring(8);\n }\n return bytes;\n}\n\nfunction unpackIEEE754(bytes, ebits, fbits) {\n\n // Bytes to bits\n var bits = [], i, j, b, str,\n bias, s, e, f;\n\n for (i = bytes.length; i; i -= 1) {\n b = bytes[i - 1];\n for (j = 8; j; j -= 1) {\n bits.push(b % 2 ? 1 : 0); b = b >> 1;\n }\n }\n bits.reverse();\n str = bits.join('');\n\n // Unpack sign, exponent, fraction\n bias = (1 << (ebits - 1)) - 1;\n s = parseInt(str.substring(0, 1), 2) ? -1 : 1;\n e = parseInt(str.substring(1, 1 + ebits), 2);\n f = parseInt(str.substring(1 + ebits), 2);\n\n // Produce number\n if (e === (1 << ebits) - 1) {\n return f !== 0 ? NaN : s * Infinity;\n } else if (e > 0) {\n // Normalized\n return s * pow(2, e - bias) * (1 + f / pow(2, fbits));\n } else if (f !== 0) {\n // Denormalized\n return s * pow(2, -(bias - 1)) * (f / pow(2, fbits));\n } else {\n return s < 0 ? -0 : 0;\n }\n}\n\nfunction unpackF64(b) { return unpackIEEE754(b, 11, 52); }\nfunction packF64(v) { return packIEEE754(v, 11, 52); }\nfunction unpackF32(b) { return unpackIEEE754(b, 8, 23); }\nfunction packF32(v) { return packIEEE754(v, 8, 23); }\n\n\n//\n// 3 The ArrayBuffer Type\n//\n\n(function() {\n\n /** @constructor */\n var ArrayBuffer = function ArrayBuffer(length) {\n length = ECMAScript.ToInt32(length);\n if (length < 0) throw new RangeError('ArrayBuffer size is not a small enough positive integer');\n\n this.byteLength = length;\n this._bytes = [];\n this._bytes.length = length;\n\n var i;\n for (i = 0; i < this.byteLength; i += 1) {\n this._bytes[i] = 0;\n }\n\n configureProperties(this);\n };\n\n exports.ArrayBuffer = exports.ArrayBuffer || ArrayBuffer;\n\n //\n // 4 The ArrayBufferView Type\n //\n\n // NOTE: this constructor is not exported\n /** @constructor */\n var ArrayBufferView = function ArrayBufferView() {\n //this.buffer = null;\n //this.byteOffset = 0;\n //this.byteLength = 0;\n };\n\n //\n // 5 The Typed Array View Types\n //\n\n function makeConstructor(bytesPerElement, pack, unpack) {\n // Each TypedArray type requires a distinct constructor instance with\n // identical logic, which this produces.\n\n var ctor;\n ctor = function(buffer, byteOffset, length) {\n var array, sequence, i, s;\n\n if (!arguments.length || typeof arguments[0] === 'number') {\n // Constructor(unsigned long length)\n this.length = ECMAScript.ToInt32(arguments[0]);\n if (length < 0) throw new RangeError('ArrayBufferView size is not a small enough positive integer');\n\n this.byteLength = this.length * this.BYTES_PER_ELEMENT;\n this.buffer = new ArrayBuffer(this.byteLength);\n this.byteOffset = 0;\n } else if (typeof arguments[0] === 'object' && arguments[0].constructor === ctor) {\n // Constructor(TypedArray array)\n array = arguments[0];\n\n this.length = array.length;\n this.byteLength = this.length * this.BYTES_PER_ELEMENT;\n this.buffer = new ArrayBuffer(this.byteLength);\n this.byteOffset = 0;\n\n for (i = 0; i < this.length; i += 1) {\n this._setter(i, array._getter(i));\n }\n } else if (typeof arguments[0] === 'object' &&\n !(arguments[0] instanceof ArrayBuffer || ECMAScript.Class(arguments[0]) === 'ArrayBuffer')) {\n // Constructor(sequence array)\n sequence = arguments[0];\n\n this.length = ECMAScript.ToUint32(sequence.length);\n this.byteLength = this.length * this.BYTES_PER_ELEMENT;\n this.buffer = new ArrayBuffer(this.byteLength);\n this.byteOffset = 0;\n\n for (i = 0; i < this.length; i += 1) {\n s = sequence[i];\n this._setter(i, Number(s));\n }\n } else if (typeof arguments[0] === 'object' &&\n (arguments[0] instanceof ArrayBuffer || ECMAScript.Class(arguments[0]) === 'ArrayBuffer')) {\n // Constructor(ArrayBuffer buffer,\n // optional unsigned long byteOffset, optional unsigned long length)\n this.buffer = buffer;\n\n this.byteOffset = ECMAScript.ToUint32(byteOffset);\n if (this.byteOffset > this.buffer.byteLength) {\n throw new RangeError(\"byteOffset out of range\");\n }\n\n if (this.byteOffset % this.BYTES_PER_ELEMENT) {\n // The given byteOffset must be a multiple of the element\n // size of the specific type, otherwise an exception is raised.\n throw new RangeError(\"ArrayBuffer length minus the byteOffset is not a multiple of the element size.\");\n }\n\n if (arguments.length < 3) {\n this.byteLength = this.buffer.byteLength - this.byteOffset;\n\n if (this.byteLength % this.BYTES_PER_ELEMENT) {\n throw new RangeError(\"length of buffer minus byteOffset not a multiple of the element size\");\n }\n this.length = this.byteLength / this.BYTES_PER_ELEMENT;\n } else {\n this.length = ECMAScript.ToUint32(length);\n this.byteLength = this.length * this.BYTES_PER_ELEMENT;\n }\n\n if ((this.byteOffset + this.byteLength) > this.buffer.byteLength) {\n throw new RangeError(\"byteOffset and length reference an area beyond the end of the buffer\");\n }\n } else {\n throw new TypeError(\"Unexpected argument type(s)\");\n }\n\n this.constructor = ctor;\n\n configureProperties(this);\n makeArrayAccessors(this);\n };\n\n ctor.prototype = new ArrayBufferView();\n ctor.prototype.BYTES_PER_ELEMENT = bytesPerElement;\n ctor.prototype._pack = pack;\n ctor.prototype._unpack = unpack;\n ctor.BYTES_PER_ELEMENT = bytesPerElement;\n\n // getter type (unsigned long index);\n ctor.prototype._getter = function(index) {\n if (arguments.length < 1) throw new SyntaxError(\"Not enough arguments\");\n\n index = ECMAScript.ToUint32(index);\n if (index >= this.length) {\n return undefined;\n }\n\n var bytes = [], i, o;\n for (i = 0, o = this.byteOffset + index * this.BYTES_PER_ELEMENT;\n i < this.BYTES_PER_ELEMENT;\n i += 1, o += 1) {\n bytes.push(this.buffer._bytes[o]);\n }\n return this._unpack(bytes);\n };\n\n // NONSTANDARD: convenience alias for getter: type get(unsigned long index);\n ctor.prototype.get = ctor.prototype._getter;\n\n // setter void (unsigned long index, type value);\n ctor.prototype._setter = function(index, value) {\n if (arguments.length < 2) throw new SyntaxError(\"Not enough arguments\");\n\n index = ECMAScript.ToUint32(index);\n if (index >= this.length) {\n return undefined;\n }\n\n var bytes = this._pack(value), i, o;\n for (i = 0, o = this.byteOffset + index * this.BYTES_PER_ELEMENT;\n i < this.BYTES_PER_ELEMENT;\n i += 1, o += 1) {\n this.buffer._bytes[o] = bytes[i];\n }\n };\n\n // void set(TypedArray array, optional unsigned long offset);\n // void set(sequence array, optional unsigned long offset);\n ctor.prototype.set = function(index, value) {\n if (arguments.length < 1) throw new SyntaxError(\"Not enough arguments\");\n var array, sequence, offset, len,\n i, s, d,\n byteOffset, byteLength, tmp;\n\n if (typeof arguments[0] === 'object' && arguments[0].constructor === this.constructor) {\n // void set(TypedArray array, optional unsigned long offset);\n array = arguments[0];\n offset = ECMAScript.ToUint32(arguments[1]);\n\n if (offset + array.length > this.length) {\n throw new RangeError(\"Offset plus length of array is out of range\");\n }\n\n byteOffset = this.byteOffset + offset * this.BYTES_PER_ELEMENT;\n byteLength = array.length * this.BYTES_PER_ELEMENT;\n\n if (array.buffer === this.buffer) {\n tmp = [];\n for (i = 0, s = array.byteOffset; i < byteLength; i += 1, s += 1) {\n tmp[i] = array.buffer._bytes[s];\n }\n for (i = 0, d = byteOffset; i < byteLength; i += 1, d += 1) {\n this.buffer._bytes[d] = tmp[i];\n }\n } else {\n for (i = 0, s = array.byteOffset, d = byteOffset;\n i < byteLength; i += 1, s += 1, d += 1) {\n this.buffer._bytes[d] = array.buffer._bytes[s];\n }\n }\n } else if (typeof arguments[0] === 'object' && typeof arguments[0].length !== 'undefined') {\n // void set(sequence array, optional unsigned long offset);\n sequence = arguments[0];\n len = ECMAScript.ToUint32(sequence.length);\n offset = ECMAScript.ToUint32(arguments[1]);\n\n if (offset + len > this.length) {\n throw new RangeError(\"Offset plus length of array is out of range\");\n }\n\n for (i = 0; i < len; i += 1) {\n s = sequence[i];\n this._setter(offset + i, Number(s));\n }\n } else {\n throw new TypeError(\"Unexpected argument type(s)\");\n }\n };\n\n // TypedArray subarray(long begin, optional long end);\n ctor.prototype.subarray = function(start, end) {\n function clamp(v, min, max) { return v < min ? min : v > max ? max : v; }\n\n start = ECMAScript.ToInt32(start);\n end = ECMAScript.ToInt32(end);\n\n if (arguments.length < 1) { start = 0; }\n if (arguments.length < 2) { end = this.length; }\n\n if (start < 0) { start = this.length + start; }\n if (end < 0) { end = this.length + end; }\n\n start = clamp(start, 0, this.length);\n end = clamp(end, 0, this.length);\n\n var len = end - start;\n if (len < 0) {\n len = 0;\n }\n\n return new this.constructor(\n this.buffer, this.byteOffset + start * this.BYTES_PER_ELEMENT, len);\n };\n\n return ctor;\n }\n\n var Int8Array = makeConstructor(1, packI8, unpackI8);\n var Uint8Array = makeConstructor(1, packU8, unpackU8);\n var Uint8ClampedArray = makeConstructor(1, packU8Clamped, unpackU8);\n var Int16Array = makeConstructor(2, packI16, unpackI16);\n var Uint16Array = makeConstructor(2, packU16, unpackU16);\n var Int32Array = makeConstructor(4, packI32, unpackI32);\n var Uint32Array = makeConstructor(4, packU32, unpackU32);\n var Float32Array = makeConstructor(4, packF32, unpackF32);\n var Float64Array = makeConstructor(8, packF64, unpackF64);\n\n exports.Int8Array = exports.Int8Array || Int8Array;\n exports.Uint8Array = exports.Uint8Array || Uint8Array;\n exports.Uint8ClampedArray = exports.Uint8ClampedArray || Uint8ClampedArray;\n exports.Int16Array = exports.Int16Array || Int16Array;\n exports.Uint16Array = exports.Uint16Array || Uint16Array;\n exports.Int32Array = exports.Int32Array || Int32Array;\n exports.Uint32Array = exports.Uint32Array || Uint32Array;\n exports.Float32Array = exports.Float32Array || Float32Array;\n exports.Float64Array = exports.Float64Array || Float64Array;\n}());\n\n//\n// 6 The DataView View Type\n//\n\n(function() {\n function r(array, index) {\n return ECMAScript.IsCallable(array.get) ? array.get(index) : array[index];\n }\n\n var IS_BIG_ENDIAN = (function() {\n var u16array = new(exports.Uint16Array)([0x1234]),\n u8array = new(exports.Uint8Array)(u16array.buffer);\n return r(u8array, 0) === 0x12;\n }());\n\n // Constructor(ArrayBuffer buffer,\n // optional unsigned long byteOffset,\n // optional unsigned long byteLength)\n /** @constructor */\n var DataView = function DataView(buffer, byteOffset, byteLength) {\n if (arguments.length === 0) {\n buffer = new exports.ArrayBuffer(0);\n } else if (!(buffer instanceof exports.ArrayBuffer || ECMAScript.Class(buffer) === 'ArrayBuffer')) {\n throw new TypeError(\"TypeError\");\n }\n\n this.buffer = buffer || new exports.ArrayBuffer(0);\n\n this.byteOffset = ECMAScript.ToUint32(byteOffset);\n if (this.byteOffset > this.buffer.byteLength) {\n throw new RangeError(\"byteOffset out of range\");\n }\n\n if (arguments.length < 3) {\n this.byteLength = this.buffer.byteLength - this.byteOffset;\n } else {\n this.byteLength = ECMAScript.ToUint32(byteLength);\n }\n\n if ((this.byteOffset + this.byteLength) > this.buffer.byteLength) {\n throw new RangeError(\"byteOffset and length reference an area beyond the end of the buffer\");\n }\n\n configureProperties(this);\n };\n\n function makeGetter(arrayType) {\n return function(byteOffset, littleEndian) {\n\n byteOffset = ECMAScript.ToUint32(byteOffset);\n\n if (byteOffset + arrayType.BYTES_PER_ELEMENT > this.byteLength) {\n throw new RangeError(\"Array index out of range\");\n }\n byteOffset += this.byteOffset;\n\n var uint8Array = new exports.Uint8Array(this.buffer, byteOffset, arrayType.BYTES_PER_ELEMENT),\n bytes = [], i;\n for (i = 0; i < arrayType.BYTES_PER_ELEMENT; i += 1) {\n bytes.push(r(uint8Array, i));\n }\n\n if (Boolean(littleEndian) === Boolean(IS_BIG_ENDIAN)) {\n bytes.reverse();\n }\n\n return r(new arrayType(new exports.Uint8Array(bytes).buffer), 0);\n };\n }\n\n DataView.prototype.getUint8 = makeGetter(exports.Uint8Array);\n DataView.prototype.getInt8 = makeGetter(exports.Int8Array);\n DataView.prototype.getUint16 = makeGetter(exports.Uint16Array);\n DataView.prototype.getInt16 = makeGetter(exports.Int16Array);\n DataView.prototype.getUint32 = makeGetter(exports.Uint32Array);\n DataView.prototype.getInt32 = makeGetter(exports.Int32Array);\n DataView.prototype.getFloat32 = makeGetter(exports.Float32Array);\n DataView.prototype.getFloat64 = makeGetter(exports.Float64Array);\n\n function makeSetter(arrayType) {\n return function(byteOffset, value, littleEndian) {\n\n byteOffset = ECMAScript.ToUint32(byteOffset);\n if (byteOffset + arrayType.BYTES_PER_ELEMENT > this.byteLength) {\n throw new RangeError(\"Array index out of range\");\n }\n\n // Get bytes\n var typeArray = new arrayType([value]),\n byteArray = new exports.Uint8Array(typeArray.buffer),\n bytes = [], i, byteView;\n\n for (i = 0; i < arrayType.BYTES_PER_ELEMENT; i += 1) {\n bytes.push(r(byteArray, i));\n }\n\n // Flip if necessary\n if (Boolean(littleEndian) === Boolean(IS_BIG_ENDIAN)) {\n bytes.reverse();\n }\n\n // Write them\n byteView = new exports.Uint8Array(this.buffer, byteOffset, arrayType.BYTES_PER_ELEMENT);\n byteView.set(bytes);\n };\n }\n\n DataView.prototype.setUint8 = makeSetter(exports.Uint8Array);\n DataView.prototype.setInt8 = makeSetter(exports.Int8Array);\n DataView.prototype.setUint16 = makeSetter(exports.Uint16Array);\n DataView.prototype.setInt16 = makeSetter(exports.Int16Array);\n DataView.prototype.setUint32 = makeSetter(exports.Uint32Array);\n DataView.prototype.setInt32 = makeSetter(exports.Int32Array);\n DataView.prototype.setFloat32 = makeSetter(exports.Float32Array);\n DataView.prototype.setFloat64 = makeSetter(exports.Float64Array);\n\n exports.DataView = exports.DataView || DataView;\n\n}());\n\n\n//# sourceURL=webpack://electron-test/./node_modules/typedarray/index.js?"); - -/***/ }), - -/***/ "./node_modules/util-deprecate/node.js": -/*!*********************************************!*\ - !*** ./node_modules/util-deprecate/node.js ***! - \*********************************************/ -/***/ ((module, __unused_webpack_exports, __webpack_require__) => { - -eval("\n/**\n * For Node.js, simply re-export the core `util.deprecate` function.\n */\n\nmodule.exports = __webpack_require__(/*! util */ \"util\").deprecate;\n\n\n//# sourceURL=webpack://electron-test/./node_modules/util-deprecate/node.js?"); - -/***/ }), - -/***/ "./node_modules/xtend/immutable.js": -/*!*****************************************!*\ - !*** ./node_modules/xtend/immutable.js ***! - \*****************************************/ -/***/ ((module) => { - -eval("module.exports = extend\n\nvar hasOwnProperty = Object.prototype.hasOwnProperty;\n\nfunction extend() {\n var target = {}\n\n for (var i = 0; i < arguments.length; i++) {\n var source = arguments[i]\n\n for (var key in source) {\n if (hasOwnProperty.call(source, key)) {\n target[key] = source[key]\n }\n }\n }\n\n return target\n}\n\n\n//# sourceURL=webpack://electron-test/./node_modules/xtend/immutable.js?"); - -/***/ }), - -/***/ "buffer": -/*!*************************!*\ - !*** external "buffer" ***! - \*************************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("buffer");; - -/***/ }), - -/***/ "child_process": -/*!********************************!*\ - !*** external "child_process" ***! - \********************************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("child_process");; - -/***/ }), - -/***/ "electron": -/*!***************************!*\ - !*** external "electron" ***! - \***************************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("electron");; - -/***/ }), - -/***/ "events": -/*!*************************!*\ - !*** external "events" ***! - \*************************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("events");; - -/***/ }), - -/***/ "net": -/*!**********************!*\ - !*** external "net" ***! - \**********************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("net");; - -/***/ }), - -/***/ "os": -/*!*********************!*\ - !*** external "os" ***! - \*********************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("os");; - -/***/ }), - -/***/ "path": -/*!***********************!*\ - !*** external "path" ***! - \***********************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("path");; - -/***/ }), - -/***/ "stream": -/*!*************************!*\ - !*** external "stream" ***! - \*************************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("stream");; - -/***/ }), - -/***/ "util": -/*!***********************!*\ - !*** external "util" ***! - \***********************/ -/***/ ((module) => { - -"use strict"; -module.exports = require("util");; - -/***/ }) - -/******/ }); -/************************************************************************/ -/******/ // The module cache -/******/ var __webpack_module_cache__ = {}; -/******/ -/******/ // The require function -/******/ function __webpack_require__(moduleId) { -/******/ // Check if module is in cache -/******/ var cachedModule = __webpack_module_cache__[moduleId]; -/******/ if (cachedModule !== undefined) { -/******/ return cachedModule.exports; -/******/ } -/******/ // Create a new module (and put it into the cache) -/******/ var module = __webpack_module_cache__[moduleId] = { -/******/ // no module.id needed -/******/ // no module.loaded needed -/******/ exports: {} -/******/ }; -/******/ -/******/ // Execute the module function -/******/ __webpack_modules__[moduleId].call(module.exports, module, module.exports, __webpack_require__); -/******/ -/******/ // Return the exports of the module -/******/ return module.exports; -/******/ } -/******/ -/************************************************************************/ -/******/ -/******/ // startup -/******/ // Load entry module and return exports -/******/ // This entry module is referenced by other modules so it can't be inlined -/******/ var __webpack_exports__ = __webpack_require__("./main.ts"); -/******/ -/******/ })() -; \ No newline at end of file diff --git a/surface/dist/img/tools.png b/surface/dist/img/tools.png new file mode 100644 index 0000000..09c7677 Binary files /dev/null and b/surface/dist/img/tools.png differ diff --git a/surface/dist/index.html b/surface/dist/index.html index 9b9488f..177bb7a 100644 --- a/surface/dist/index.html +++ b/surface/dist/index.html @@ -1,10 +1 @@ - - - - - ROV Triton Interface - - -
- - \ No newline at end of file +ROV Triton Interface
\ No newline at end of file diff --git a/surface/electron/com.ts b/surface/electron/com.ts new file mode 100644 index 0000000..f75e698 --- /dev/null +++ b/surface/electron/com.ts @@ -0,0 +1,46 @@ +import path from 'path'; +import {spawn} from 'child_process'; +import msg, { LOG_ERROR, LOG_SUCCESS } from '../src/components/Log/LogItem'; +import {COM} from '../src/components/Log/channels'; +import net from 'net'; +import {ipcMain} from 'electron'; + +const messager = (win) => { + let sock = net.connect(11003, undefined, () => { + win.webContents.send(COM, msg('com', 'Socket connected', LOG_SUCCESS)); + }); + + ipcMain.on('com_send', (e, data: Array,) => { + try{ + let str = ''; + for(let v of data) str += `${v.toString()},`; + str = str.slice(0, -1); + str += ';'; + sock.write(str); + }catch(e){ + win.webContents.send(COM, msg('com', `Error: ${e}`, LOG_ERROR)); + } + }) + + win.on('close', sock.end); +} + +const com = async (win) => { + let sender = spawn('python3', ['-u', path.resolve(__dirname, '../ros/src/com_pub/src/sender.py'), '11003']); + + sender.stderr.on('data', e => win.webContents.send(COM, msg('com', `Error: ${e}`))); + + sender.stdout.on('data', data => { + win.webContents.send(COM, msg('com', `Data: ${data}`)) + if(`${data}`.includes('ready')){ + messager(win); + } + }) + + win.on('close', _ => { + sender.kill('SIGINT'); + sender.kill('SIGINT'); + }) +} + +export default com; \ No newline at end of file diff --git a/surface/electron/gamepad.ts b/surface/electron/gamepad.ts index cb4fef5..7aa0e1f 100644 --- a/surface/electron/gamepad.ts +++ b/surface/electron/gamepad.ts @@ -1,37 +1,124 @@ import path from 'path'; import {spawn} from 'child_process'; -import msg from '../src/components/Log/LogItem'; +import msg, { LOG_ERROR, LOG_SUCCESS, LOG_WARNING } from '../src/components/Log/LogItem'; import {GAMEPAD} from '../src/components/Log/channels'; +import net from 'net'; +import { ipcMain } from 'electron'; -let timeout = 0; +export interface GamepadParams { + type: 'trim' | 'scale' | 'reverse' | 'absolute' | 'lockout' | 'mode' + reverse?: string + mode?: string + lockout?: string + values?: Array +} -export async function gamepadListener(win) { - let sender = spawn('python3', ['-u', path.resolve(__dirname, '../ros/src/gamepad/src/sender.py')]); - timeout = Date.now(); +let socket = new net.Socket(); - sender.on('exit', code => { - if(Date.now() - timeout > 1000){ - win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Error! Gamepad lost. Reacquiring...')); - } - setTimeout(() => { - gamepadListener(win); - }, 1000) +let sender; + +const sendParams = (win, socket, params: GamepadParams) => { + try{ + let str = `${params.type}:${params.reverse!}` + socket.write(str); + }catch(e){ + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket write error', LOG_ERROR)); + } +} + +const wait = async (win) => { + let promise = new Promise((resolve, reject) => { + setTimeout(() => resolve('spawn again'), 1000); + }); + await promise; + + gamepadListener(win); +} + +const kill = () => { + sender.kill('SIGINT'); + sender.kill('SIGINT'); +} + +const gamepadListener = async (win: Electron.BrowserWindow) => { + win.on('close', kill); + + sender = spawn('python3', ['-u', path.resolve(__dirname, '../ros/src/gamepad/src/sender.py')]); + + sender.on('exit', _ => { + wait(win); + win.removeListener('close', kill); }); sender.stderr.on('data', data => { - console.log(`Gamepad Listener:\n${data}`); - win.webContents.send(GAMEPAD, msg('gamepad_listener', `Error: ${data}`)); - }) + win.webContents.send(GAMEPAD, msg('gamepad_listener', `Error: ${data}`, LOG_ERROR)); + }); sender.stdout.on('data', data => { - console.log(data.toString()); if(`${data}`.includes('ready')){ - win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Gamepad connected')); + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Gamepad connected', LOG_SUCCESS)); + socket = net.connect(11001, 'localhost', () => { + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket connected', LOG_SUCCESS)); + }) + + socket.on('error', (err) => { + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket error', LOG_ERROR)); + }) + + ipcMain.on('gamepad_sock', (e, params: GamepadParams) => { + try{ + let str = `${params.type}:` + for(let v of params.values!) str += `${v.toString()},`; + str = str.slice(0, -1); + socket.write(str); + }catch(e){ + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket write error', LOG_ERROR)); + } + }) + + ipcMain.on('compensator', (e, params: GamepadParams) => { + try{ + let str = `${params.type}:` + for(let v of params.values!) str += `${v.toString()},`; + str = str.slice(0, -1); + socket.write(str); + }catch(e){ + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket write error', LOG_ERROR)); + } + }) + + ipcMain.on('reverse', (e, params: GamepadParams) => sendParams(win, socket, params)); + + ipcMain.on('lockout', (e, params: GamepadParams) => { + try{ + let str = `${params.type}:${params.lockout}`; + socket.write(str); + }catch(e){ + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket write error', LOG_ERROR)); + } + }); + + ipcMain.on('mode', (e, params: GamepadParams) => { + try{ + let str = `${params.type}:${params.mode}`; + socket.write(str); + }catch(e){ + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket write error', LOG_ERROR)); + } + }); + + ipcMain.on('absolute', (e, params: GamepadParams) => { + try{ + let str = `${params.type}:${params.values![0]}`; + socket.write(str); + }catch(e){ + win.webContents.send(GAMEPAD, msg('gamepad_listener', 'Socket write error', LOG_ERROR)); + } + }); } - }) - win.on('close', _ => { - sender.kill('SIGINT'); - sender.kill('SIGINT'); + win.webContents.send(GAMEPAD, msg('gamepad_listener', `Data: ${data}`, LOG_WARNING)); }) -} \ No newline at end of file +} + +export default gamepadListener; \ No newline at end of file diff --git a/surface/electron/imu.ts b/surface/electron/imu.ts index 79fc1e5..70cc273 100644 --- a/surface/electron/imu.ts +++ b/surface/electron/imu.ts @@ -1,7 +1,7 @@ import path from 'path'; import {spawn} from 'child_process'; import {IMU} from '../src/components/Log/channels'; -import msg from '../src/components/Log/LogItem'; +import msg, { LOG_ERROR, LOG_WARNING } from '../src/components/Log/LogItem'; export default async function imu(win) { let listener = spawn('python3', ['-u', path.resolve(__dirname, '../ros/src/imu/src/status.py')]); @@ -9,7 +9,7 @@ export default async function imu(win) { win.webContents.send(IMU, msg('imu', 'Started node')); listener.on('exit', code => { - win.webContents.send(IMU, msg('imu', 'Node exited')); + win.webContents.send(IMU, msg('imu', 'Node exited', LOG_WARNING)); }); listener.stdout.on('data', data => { @@ -23,7 +23,7 @@ export default async function imu(win) { }) listener.stderr.on('data', data => { - win.webContents.send(IMU, msg('imu', `Error: ${data}`)); + win.webContents.send(IMU, msg('imu', `Error: ${data}`, LOG_ERROR)); }) win.on('close', _ => { diff --git a/surface/electron/seqimgr.ts b/surface/electron/seqimgr.ts new file mode 100644 index 0000000..2880d4d --- /dev/null +++ b/surface/electron/seqimgr.ts @@ -0,0 +1,44 @@ +import path from 'path'; +import {spawn} from 'child_process'; +import msg, { LOG_ERROR, LOG_SUCCESS } from '../src/components/Log/LogItem'; +import {RAILCAP} from '../src/components/Log/channels'; +import net from 'net'; +import {ipcMain} from 'electron'; + +const messager = (win) => { + let sock = net.connect(11005, undefined, () => { + win.webContents.send(RAILCAP, msg('frame_taker', 'Socket connected', LOG_SUCCESS)); + }); + + ipcMain.on('take_frame', (e, count: number) => { + try{ + console.log('writing count') + sock.write(count.toString()); + }catch(e){ + win.webContents.send(RAILCAP, msg('frame_taker', `Error: ${e}`, LOG_ERROR)); + } + }) + + win.on('close', sock.end); +} + +const seqimgr = async (win) => { + let sender = spawn('python3', ['-u', path.resolve(__dirname, '../cv/stream.py'), '11005']); + + sender.stderr.on('data', e => win.webContents.send(RAILCAP, msg('frame_taker', `Error: ${e}`))); + + sender.stdout.on('data', data => { + win.webContents.send(RAILCAP, msg('frame_taker', `Data: ${data}`)) + console.log(`${data}`); + if(`${data}`.includes('ready')){ + messager(win); + } + }) + + win.on('close', _ => { + sender.kill('SIGINT'); + sender.kill('SIGINT'); + }) +} + +export default seqimgr; \ No newline at end of file diff --git a/surface/electron/servo.ts b/surface/electron/servo.ts index 2943f01..15036bc 100644 --- a/surface/electron/servo.ts +++ b/surface/electron/servo.ts @@ -1,48 +1,47 @@ import path from 'path'; import {spawn} from 'child_process'; -import msg from '../src/components/Log/LogItem'; +import msg, { LOG_ERROR, LOG_SUCCESS, LOG_WARNING } from '../src/components/Log/LogItem'; import {SERVO} from '../src/components/Log/channels'; import net from 'net'; import {ipcMain} from 'electron'; -function send(win){ +const messager = (win) => { let sock = net.connect(11002, undefined, () => { - win.webContents.send(SERVO, msg('servo', 'Socket connected')); + win.webContents.send(SERVO, msg('servo', 'Socket connected', LOG_SUCCESS)); }); ipcMain.on('servo_send', (e, data: number,) => { sock.write(`${data}`); }) - sock.on('error', _ => win.webContents.send(SERVO, msg('servo', 'Socket error!'))); + sock.on('error', _ => win.webContents.send(SERVO, msg('servo', 'Socket error!', LOG_ERROR))); - win.on('close', _ => { - sock.end(); - }) + win.on('close', sock.end); } -export default async function servo(win) { +const servo = async (win) => { let sender = spawn('python3', ['-u', path.resolve(__dirname, '../ros/src/cam_servo/src/servo_sender.py'), '11002']); win.webContents.send(SERVO, msg('servo', 'Started')); - sender.on('exit', code => { - win.webContents.send(SERVO, msg('servo', 'Exiting...')); + win.webContents.send(SERVO, msg('servo', 'Exiting...', LOG_WARNING)); }); sender.stdout.on('data', data => { if(`${data}`.includes('ready')){ - send(win); + messager(win); } }) sender.stderr.on('data', data => { - console.log(`Gamepad Listener:\n${data}`); - win.webContents.send(SERVO, msg('servo', `Error: ${data}`)); + win.webContents.send(SERVO, msg('servo', `Error: ${data}`, LOG_ERROR)); }) win.on('close', _ => { sender.kill('SIGINT'); + sender.kill('SIGINT'); }) -} \ No newline at end of file +} + +export default servo; \ No newline at end of file diff --git a/surface/electron/setIp.ts b/surface/electron/setIp.ts index 46a9ecb..671e68c 100644 --- a/surface/electron/setIp.ts +++ b/surface/electron/setIp.ts @@ -1,6 +1,8 @@ import os from 'os'; +import { SET_IP } from '../src/components/Log/channels'; +import log from '../src/components/Log/LogItem'; -async function getIp() { +const getIp = () => { let inter = os.networkInterfaces(); let i: Array @@ -15,12 +17,14 @@ async function getIp() { return 'localhost'; } -export default async function setIp(){ - const ip = await getIp(); +const setIp = async (win) => { + const ip = getIp(); process.env.ROS_HOSTNAME = ip; process.env.ROS_IP = ip; process.env.ROS_MASTER_URI = 'http://192.168.1.3:11311'; - return ip; -} \ No newline at end of file + win.webContents.send(SET_IP, log('ip_set', `Found ${ip} as local machine`)); +} + +export default setIp; \ No newline at end of file diff --git a/surface/electron/setupRos.ts b/surface/electron/setupRos.ts index bc86c3e..b084ba2 100644 --- a/surface/electron/setupRos.ts +++ b/surface/electron/setupRos.ts @@ -11,13 +11,14 @@ const sourceRos = async () => { }) } -export default async function setupRos() { +const setupRos = async () => { process.chdir('./ros'); - const stdout = execSync('catkin_make'); + + execSync('catkin_make'); - let env: NodeJS.ProcessEnv = await sourceRos(); + await sourceRos(); process.chdir('../'); +} - return env; -} \ No newline at end of file +export default setupRos; \ No newline at end of file diff --git a/surface/electron/thrusters.ts b/surface/electron/thrusters.ts index f858c90..82a1b3e 100644 --- a/surface/electron/thrusters.ts +++ b/surface/electron/thrusters.ts @@ -1,16 +1,15 @@ import path from 'path'; import {spawn} from 'child_process'; -import {ipcMain} from 'electron'; import {THRUSTERS} from '../src/components/Log/channels'; -import msg from '../src/components/Log/LogItem'; +import msg, { LOG_ERROR, LOG_WARNING } from '../src/components/Log/LogItem'; -export default async function thrusters(win) { +const thrusters = async (win) => { let listener = spawn('python3', ['-u', path.resolve(__dirname, '../ros/src/thrusters/src/status.py')]); - win.webContents.send(THRUSTERS, msg('thrusters', 'Started node')); + win.webContents.send(THRUSTERS, msg(THRUSTERS, 'Started node')); listener.on('exit', code => { - win.webContents.send(THRUSTERS, msg('thrusters', 'Node exited')); + win.webContents.send(THRUSTERS, msg(THRUSTERS, 'Node exited', LOG_WARNING)); }); listener.stdout.on('data', data => { @@ -24,10 +23,12 @@ export default async function thrusters(win) { }) listener.stderr.on('data', data => { - win.webContents.send(THRUSTERS, msg('thrusters', `Error: ${data}`)); + win.webContents.send(THRUSTERS, msg(THRUSTERS, `Error: ${data}`, LOG_ERROR)); }) win.on('close', _ => { listener.kill('SIGINT'); }) -} \ No newline at end of file +} + +export default thrusters; \ No newline at end of file diff --git a/surface/main.ts b/surface/main.ts index 66bb97d..96618a9 100644 --- a/surface/main.ts +++ b/surface/main.ts @@ -1,36 +1,42 @@ import {app, BrowserWindow , ipcMain} from 'electron'; import setIp from './electron/setIp'; import setupRos from './electron/setupRos'; -import {gamepadListener} from './electron/gamepad'; -import log from './src/components/Log/LogItem'; -import {CATKIN_MAKE, SET_IP} from './src/components/Log/channels'; +import gamepadListener from './electron/gamepad'; +import log, { LOG_SUCCESS } from './src/components/Log/LogItem'; +import {CATKIN_MAKE, GENERAL, IMU, SERVO, SET_IP, THRUSTERS, RAILCAP} from './src/components/Log/channels'; import servo from './electron/servo'; import thrusters from './electron/thrusters'; import imu from './electron/imu'; +import com from './electron/com'; +import seqimgr from './electron/seqimgr'; +import path from 'path'; +import {spawn} from 'child_process'; -const nodeManager = async (win) => { - await setIp().then((addr) => { - win.webContents.send(SET_IP, log('ip_set', `Found ${addr} as local machine`)); - }); - setupRos().then((env) => { - process.env = env; - gamepadListener(win); - win.webContents.send(CATKIN_MAKE, log('catkin_make', 'Built and sourced')); - servo(win).catch(e => { - console.log(e); - }); - thrusters(win).catch(e => { - console.log(e); - }); - imu(win).catch(e => { - console.log(e); - }) - }).catch((err) => { - win.webContents.send(CATKIN_MAKE, log('catkin_make', `Error: ${err}`)); +const nodeManager = async (win: BrowserWindow) => { + + await setIp(win).catch(e => win.webContents.send(SET_IP, log('SetIP', `Error: ${e}`))); + + await setupRos().catch(e => win.webContents.send(CATKIN_MAKE, log('catkin_make', `Error: ${e}`))); + + gamepadListener(win); + win.webContents.send(CATKIN_MAKE, log('catkin_make', 'Built and sourced')); + + servo(win).catch(e => win.webContents.send(SERVO, `Error: ${e}`)); + + thrusters(win).catch(e => win.webContents.send(THRUSTERS, log('Thrusters', `Error: ${e}`))); + + seqimgr(win).catch(e => win.webContents.send(RAILCAP, log('rail_cap', `Error: ${e}`))); + + imu(win).catch(e => win.webContents.send(IMU, log('IMU', `Error: ${e}`))); + + com(win); + + ipcMain.on('process_frame', (e) => { + spawn('python3', ['-u', path.resolve(__dirname, 'cv/process.py')]); }) } -function createWindow () { +const createWindow = () => { // Create the browser window. let win = new BrowserWindow({ width: 1920, @@ -44,12 +50,17 @@ function createWindow () { // and load the index.html of the app. win.loadFile('./index.html'); - //win.removeMenu(); ipcMain.on('logger', (e, msg) => { - if(msg == 'ready') nodeManager(win); + e.sender.send(GENERAL, log('General', 'Welcome!', LOG_SUCCESS)); + e.sender.send(GENERAL, log('General', 'Pilot Interface Starting...')); }) - + + if(process.env.NODE_ENV !== 'development'){ + ipcMain.on('logger', (e, msg) => { + if(msg == 'ready') nodeManager(win); + }); + } } app.on('ready', createWindow); diff --git a/surface/package.json b/surface/package.json index 1fd49a6..eed58cb 100644 --- a/surface/package.json +++ b/surface/package.json @@ -6,7 +6,7 @@ "scripts": { "start": "electron ./dist/electron.js", "watch": "webpack --watch --mode development", - "build": "webpack --mode development" + "build": "webpack --mode production" }, "author": "", "license": "ISC", diff --git a/surface/ros/src/cam_servo/src/servo_sender.py b/surface/ros/src/cam_servo/src/servo_sender.py index ce9daac..84af51d 100755 --- a/surface/ros/src/cam_servo/src/servo_sender.py +++ b/surface/ros/src/cam_servo/src/servo_sender.py @@ -2,6 +2,7 @@ import rospy from std_msgs.msg import Float32, String +from shared_msgs.msg import servo_msg import socket, signal, sys import threading @@ -16,8 +17,6 @@ def __init__(self, port): self.sock.settimeout(1) self.connected = False - print('ready') - self.thread = threading.Thread(target=self.run) self.thread.start() @@ -43,23 +42,29 @@ def run(self): angle = float(data.decode()) + msg = servo_msg() + msg.angle = angle + msg.servo_lock_status = False + msg.header.stamp = rospy.Time.now() + pub.publish(msg) + def shutdown(sig, frame): global sock - sock.shutdown() - rospy.signal_shutdown('interrupt') + print('please') + sock.shutdown() + rospy.signal_shutdown('now') if __name__ == '__main__': signal.signal(signal.SIGINT, shutdown) signal.signal(signal.SIGTERM, shutdown) - rospy.init_node('servo_sender') - sock = SocketManager(int(sys.argv[1])) - pub = rospy.Publisher('/rov/servo', Float32, queue_size=10) - rate = rospy.Rate(100) + rospy.init_node('servo_sender', disable_signals=True) + + pub = rospy.Publisher('/rov/ServoAngles', servo_msg, queue_size=10) + + print('ready') - while not rospy.is_shutdown(): - pub.publish(angle) - rate.sleep() + rospy.spin() diff --git a/ros/src/roslib_comm/CMakeLists.txt b/surface/ros/src/com_pub/CMakeLists.txt similarity index 96% rename from ros/src/roslib_comm/CMakeLists.txt rename to surface/ros/src/com_pub/CMakeLists.txt index 1e672f8..5fca159 100644 --- a/ros/src/roslib_comm/CMakeLists.txt +++ b/surface/ros/src/com_pub/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 2.8.3) -project(roslib_comm) +cmake_minimum_required(VERSION 3.0.2) +project(com_pub) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES roslib +# LIBRARIES cam_servo # CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) @@ -122,7 +122,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/roslib_comm.cpp +# src/${PROJECT_NAME}/cam_servo.cpp # ) ## Add cmake target dependencies of the library @@ -133,7 +133,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/roslib_comm_node.cpp) +# add_executable(${PROJECT_NAME}_node src/cam_servo_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -159,7 +159,7 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -# install(PROGRAMS +# catkin_install_python(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) @@ -197,7 +197,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_roslib_comm.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_cam_servo.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/surface/ros/src/com_pub/package.xml b/surface/ros/src/com_pub/package.xml new file mode 100644 index 0000000..d0bbe1a --- /dev/null +++ b/surface/ros/src/com_pub/package.xml @@ -0,0 +1,68 @@ + + + com_pub + 0.0.0 + The com_pub package + + + + + eric + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/surface/ros/src/com_pub/src/sender.py b/surface/ros/src/com_pub/src/sender.py new file mode 100755 index 0000000..78f1e52 --- /dev/null +++ b/surface/ros/src/com_pub/src/sender.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +import rospy +from std_msgs.msg import Float32, String +from shared_msgs.msg import com_msg +import socket, signal, sys +import threading + +com_x = 0.0 +com_y = 0.0 +com_z = 0.0 + +class SocketManager: + def __init__(self, port): + self.running = True + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sock.bind(('127.0.0.1', port)) + self.sock.listen(5) + self.sock.settimeout(1) + self.connected = False + + self.thread = threading.Thread(target=self.run) + self.thread.start() + + def shutdown(self): + self.running = False + self.sock.close() + self.thread.join() + + def run(self): + while not self.connected and self.running: + try: + conn, addr = self.sock.accept() + self.connected = True + except: + pass + while self.running: + try: + data = conn.recv(25) + except: + pass + if data: + global com_x, com_y, com_z, pub + + arr = [float(d) for d in data.decode().split(';')[0].split(',')] + + if len(arr) == 3: + com_x = arr[0] + com_y = arr[1] + com_z = arr[2] + + msg = com_msg() + msg.com[0] = com_x + msg.com[1] = com_y + msg.com[2] = com_z + pub.publish(msg) + +def shutdown(sig, frame): + global sock + + print('please') + sock.shutdown() + rospy.signal_shutdown('now') + +if __name__ == '__main__': + signal.signal(signal.SIGINT, shutdown) + signal.signal(signal.SIGTERM, shutdown) + + sock = SocketManager(int(sys.argv[1])) + + rospy.init_node('com_sender', disable_signals=True) + + pub = rospy.Publisher('/rov/com_tweak', com_msg, queue_size=10) + rate = rospy.Rate(10) + + print('ready') + + rospy.spin() diff --git a/surface/ros/src/gamepad/src/config.py b/surface/ros/src/gamepad/src/config.py index 04345e0..50a6d36 100644 --- a/surface/ros/src/gamepad/src/config.py +++ b/surface/ros/src/gamepad/src/config.py @@ -1,13 +1,9 @@ MIN_ABS_DIFFERENCE = 0 TRIGGER_DEAD_ZONE = 50 -TRIGGER_RANGE = 550 +TRIGGER_RANGE = 1025 STICK_DEAD_ZONE = 4000 STICK_RANGE = 32768 -SCALE_TRANSLATIONAL = 5.0 -SCALE_TRANSLATIONAL_MAGIC = 0.5 -SCALE_ROTATIONAL = 1.5 - EVENT_MISC = 'Misc' EVENT_SYNC = 'Sync' EVENT_ABSOLUTE = 'Absolute' diff --git a/surface/ros/src/gamepad/src/sender.py b/surface/ros/src/gamepad/src/sender.py index 0f1ea86..1e3fafd 100755 --- a/surface/ros/src/gamepad/src/sender.py +++ b/surface/ros/src/gamepad/src/sender.py @@ -7,24 +7,31 @@ #ROS import rospy from std_msgs.msg import String, Bool, Empty -from shared_msgs.msg import rov_velocity_command +from shared_msgs.msg import rov_velocity_command, tools_command_msg from geometry_msgs.msg import Twist import socket, threading import signal, os from config import * -pm_toggle = False -gh_toggle = False -bs_toggle = False +tools = [False, False, False, False] -SCALE_TRANSLATIONAL_X = 4.0 -SCALE_TRANSLATIONAL_Y = 4.0 -SCALE_TRANSLATIONAL_Z = 4.0 +SCALE_TRANSLATIONAL_X = 1.0 +SCALE_TRANSLATIONAL_Y = 1.0 +SCALE_TRANSLATIONAL_Z = 1.0 -SCALE_ROTATIONAL_X = 1.5 -SCALE_ROTATIONAL_Y = 1.5 -SCALE_ROTATIONAL_Z = 1.5 +SCALE_ROTATIONAL_X = 1.0 +SCALE_ROTATIONAL_Y = 1.0 +SCALE_ROTATIONAL_Z = 1.0 + +TRIM_X = 0.0 +TRIM_Y = 0.0 +TRIM_Z = 0.0 + +REVERSE = 1 +LOCKOUT = True +MODE = True +FINE_MULTIPLIER = 1.041 class SocketManager: def __init__(self): @@ -45,6 +52,7 @@ def shutdown(self): def run(self): global SCALE_ROTATIONAL_X, SCALE_ROTATIONAL_Y, SCALE_ROTATIONAL_Z, SCALE_TRANSLATIONAL_X, SCALE_TRANSLATIONAL_Y, SCALE_TRANSLATIONAL_Z + global TRIM_X, TRIM_Y, TRIM_Z, REVERSE, LOCKOUT, MODE, FINE_MULTIPLIER while not self.connected and self.running: try: @@ -58,23 +66,40 @@ def run(self): except: pass if data: - arr = [float(d) for d in data.decode().split(';')[0].split(',')] - SCALE_TRANSLATIONAL_X = arr[0] - SCALE_TRANSLATIONAL_Y = arr[1] - SCALE_TRANSLATIONAL_Z = arr[2] - - SCALE_ROTATIONAL_X = arr[3] - SCALE_ROTATIONAL_Y = arr[4] - SCALE_ROTATIONAL_Z = arr[5] + decoded = data.decode() + mode = decoded.split(':')[0] + + if mode == 'scale': + arr = [float(d) for d in decoded.split(':')[1].split(',')] + SCALE_TRANSLATIONAL_X = arr[0] + SCALE_TRANSLATIONAL_Y = arr[1] + SCALE_TRANSLATIONAL_Z = arr[2] + + SCALE_ROTATIONAL_X = arr[3] + SCALE_ROTATIONAL_Y = arr[4] + SCALE_ROTATIONAL_Z = arr[5] + elif mode == 'trim': + arr = [float(d) for d in decoded.split(':')[1].split(',')] + TRIM_X = arr[0] + TRIM_Y = arr[1] + TRIM_Z = arr[2] + elif mode == 'reverse': + REVERSE = 1 if decoded.split(':')[1] == 'F' else -1 + elif mode == 'lockout': + LOCKOUT = decoded.split(':')[1] == 'T' + elif mode == 'mode': + MODE = decoded.split(':')[1] == 'T' + elif mode == 'absolute': + FINE_MULTIPLIER = float(decoded.split(':')[1]) def getMessage(): global gamepad_state t = Twist() - t.linear.x = gamepad_state['LSY'] * SCALE_TRANSLATIONAL_X - t.linear.y = gamepad_state['LSX'] * SCALE_TRANSLATIONAL_Y - t.linear.z = (gamepad_state['RT'] - gamepad_state['LT']) * SCALE_TRANSLATIONAL_Z + t.linear.x = -(gamepad_state['LSY'] * SCALE_TRANSLATIONAL_X + TRIM_X) * REVERSE + t.linear.y = -(gamepad_state['LSX'] * SCALE_TRANSLATIONAL_Y + TRIM_Y) * REVERSE + t.linear.z = (gamepad_state['RT'] - gamepad_state['LT']) * SCALE_TRANSLATIONAL_Z + TRIM_Z if gamepad_state['LB'] == 1: x = 1 * SCALE_ROTATIONAL_X @@ -83,26 +108,19 @@ def getMessage(): else: x = 0.0 - t.angular.x = x - t.angular.y = gamepad_state['RSY'] * SCALE_ROTATIONAL_Y + t.angular.x = -x + t.angular.y = (-gamepad_state['RSY'] * SCALE_ROTATIONAL_Y) * REVERSE t.angular.z = -gamepad_state['RSX'] * SCALE_ROTATIONAL_Z - return rov_velocity_command(t, 'gamepad', False, False) - -def getPMState(): - global pm_toggle + return rov_velocity_command(t, 'gamepad', MODE, FINE_MULTIPLIER, False, False) - return Bool(pm_toggle) +def getTools(): + global tools -def getBSState(): - global bs_toggle + tm = tools_command_msg() + tm.tools = [i for i in tools] - return Bool(bs_toggle) - -def getGHState(): - global gh_toggle - - return Bool(gh_toggle) + return tm def correct_raw(raw, abbv): sign = (raw >= 0) * 2 - 1 @@ -134,13 +152,16 @@ def process_event(event): if event.ev_type == EVENT_KEY: gamepad_state[EVENTS[event.code]] = event.state if event.code == 'BTN_SOUTH' and event.state: - pm_toggle = not pm_toggle + tools[1] = not tools[1] if event.code == 'BTN_EAST' and event.state: - gh_toggle = not gh_toggle + tools[0] = not tools[0] if event.code == 'BTN_WEST' and event.state: - bs_toggle = not bs_toggle + tools[2] = not tools[2] + + if event.code == 'BTN_NORTH' and event.state and LOCKOUT: + tools[3] = not tools[3] elif event.ev_type == EVENT_ABSOLUTE: gamepad_state[EVENTS[event.code]] = correct_raw(event.state, EVENTS[event.code]) @@ -149,9 +170,7 @@ def process_event(event): def pub_data(event): pub.publish(getMessage()) - pub_pm.publish(getPMState()) - pub_gh.publish(getGHState()) - pub_bs.publish(getBSState()) + pub_tools.publish(getTools()) def update_gamepad(event): try: @@ -165,10 +184,11 @@ def shutdown(sig, frame): print('shutting down') data_thread.shutdown() gamepad_thread.shutdown() - #sock_thread.shutdown() + sock_thread.shutdown() + rospy.signal_shutdown('now') if __name__ == '__main__': - global pub, pub_pm, pub_gh, data_thread, gamepad_thread, sock_thread + global pub, pub_tools, data_thread, gamepad_thread, sock_thread signal.signal(signal.SIGINT, shutdown) signal.signal(signal.SIGTERM, shutdown) @@ -178,14 +198,13 @@ def shutdown(sig, frame): except: sys.exit(0) - rospy.init_node('gp_pub', anonymous=True) + sock_thread = SocketManager() + + rospy.init_node('gp_pub', anonymous=True, disable_signals=True) pub = rospy.Publisher('rov_velocity', rov_velocity_command, queue_size=10) - pub_pm = rospy.Publisher('pm_cmd', Bool, queue_size=10) - pub_gh = rospy.Publisher('gh_cmd', Bool, queue_size=10) - pub_bs = rospy.Publisher('bs_cmd', Bool, queue_size=10) + pub_tools = rospy.Publisher('tools', tools_command_msg, queue_size=10) - #sock_thread = SocketManager() data_thread = rospy.Timer(rospy.Duration(0.1), pub_data) gamepad_thread = rospy.Timer(rospy.Duration(0.001), update_gamepad) @@ -196,4 +215,4 @@ def shutdown(sig, frame): data_thread.shutdown() gamepad_thread.shutdown() - #sock_thread.shutdown() \ No newline at end of file + sock_thread.shutdown() \ No newline at end of file diff --git a/surface/ros/src/gamepad/src/testSender.py b/surface/ros/src/gamepad/src/testSender.py deleted file mode 100755 index e2a7ffc..0000000 --- a/surface/ros/src/gamepad/src/testSender.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env python3 -import json -import rospy -from std_msgs.msg import String -from shared_msgs.msg import rov_velocity_command -from geometry_msgs.msg import Twist - -cmd = rov_velocity_command(Twist(), 'something', False, False) - -def pub_data(event): - pub.publish(cmd) - -def talker(): - rospy.Timer(rospy.Duration(0.1), pub_data) - - rospy.spin() - -if __name__ == '__main__': - global pub - - pub = rospy.Publisher('chatter', rov_velocity_command, queue_size=10) - rospy.init_node('test_pub', anonymous=True) - - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/surface/ros/src/ramping/src/sender.py b/surface/ros/src/ramping/src/sender.py deleted file mode 100755 index d751c12..0000000 --- a/surface/ros/src/ramping/src/sender.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 - -import json -import sys - -#ROS -import rospy -from std_msgs.msg import String - -thrust_ramp = {'ramp': 0.001} - -def pub_data(event): - #rospy.loginfo(gamepad_state) - print(json.dumps(gamepad_state)) #Proper formatting for stdout - pub.publish(json.dumps(gamepad_state)) - -def talker(): - print("Waiting") - newVal = float(input('val').rstrip()) - - thrust_ramp['ramp'] = newVal - - pub.publish(json.dumps(thrust_ramp)) - print(thrust_ramp) - -if __name__ == '__main__': - global pub - - rospy.init_node('ramp_pub', anonymous=True) - pub = rospy.Publisher('ramp', String, queue_size=10) - - r = rospy.Rate(50) - while not rospy.is_shutdown(): - talker() - r.sleep() diff --git a/surface/ros/src/shared_msgs/CMakeLists.txt b/surface/ros/src/shared_msgs/CMakeLists.txt index d574296..19b05b1 100644 --- a/surface/ros/src/shared_msgs/CMakeLists.txt +++ b/surface/ros/src/shared_msgs/CMakeLists.txt @@ -62,6 +62,7 @@ add_message_files( thrust_disable_inverted_msg.msg rov_velocity_command.msg servo_msg.msg + com_msg.msg ) ## Generate services in the 'srv' folder diff --git a/surface/ros/src/shared_msgs/msg/com_msg.msg b/surface/ros/src/shared_msgs/msg/com_msg.msg new file mode 100644 index 0000000..92b5de2 --- /dev/null +++ b/surface/ros/src/shared_msgs/msg/com_msg.msg @@ -0,0 +1 @@ +float32[3] com \ No newline at end of file diff --git a/surface/ros/src/shared_msgs/msg/rov_velocity_command.msg b/surface/ros/src/shared_msgs/msg/rov_velocity_command.msg index 8a2bff8..fd7ecaf 100644 --- a/surface/ros/src/shared_msgs/msg/rov_velocity_command.msg +++ b/surface/ros/src/shared_msgs/msg/rov_velocity_command.msg @@ -1,4 +1,6 @@ geometry_msgs/Twist twist string source +bool isFine +float32 multiplier bool isPercentPower bool isPoolCentric \ No newline at end of file diff --git a/surface/ros/src/shared_msgs/msg/thrust_command_msg.msg b/surface/ros/src/shared_msgs/msg/thrust_command_msg.msg index 7a1d3b4..0fc823f 100644 --- a/surface/ros/src/shared_msgs/msg/thrust_command_msg.msg +++ b/surface/ros/src/shared_msgs/msg/thrust_command_msg.msg @@ -1,3 +1,4 @@ float32[6] desired_thrust - +bool isFine +float32 multiplier diff --git a/surface/ros/src/shared_msgs/msg/tools_command_msg.msg b/surface/ros/src/shared_msgs/msg/tools_command_msg.msg index 275f6f6..fdd5c9d 100644 --- a/surface/ros/src/shared_msgs/msg/tools_command_msg.msg +++ b/surface/ros/src/shared_msgs/msg/tools_command_msg.msg @@ -1,3 +1 @@ -int8 pm -int8 am -int8 ghost \ No newline at end of file +int8[4] tools \ No newline at end of file diff --git a/surface/src/MainWindow.tsx b/surface/src/MainWindow.tsx index 92822de..1318b46 100644 --- a/surface/src/MainWindow.tsx +++ b/surface/src/MainWindow.tsx @@ -3,41 +3,42 @@ import Servo from './components/Servo/Servo'; import ThrusterInfo from './components/Thrusters/ThrusterInfo/ThrusterInfo'; import Log from './components/Log/Log'; import Camera from './components/Camera/Camera'; -import ThrustTweaker from './components/Thrusters/Tweaker/ThrustTweaker'; import IMU from './components/IMU/IMU'; +import Switch from './components/Switch/Switch'; +import Mosaic from './components/Mosaic/Mosaic'; +import Profiles from './components/Profiles/Profiles'; -export default class MainWindow extends React.Component<{}, {}> { - constructor(props){ - super(props); - } - render(){ - return( -
-
+const MainWindow: React.FC = () => { + return( +
+
- - + + + -
-
+
+
- + -
-
+
+
- - + + + -
+
-
- - +
+ + -
-
- ) - } -} \ No newline at end of file + + + ) +} + +export default MainWindow; \ No newline at end of file diff --git a/surface/src/components/Camera/Camera.scss b/surface/src/components/Camera/Camera.scss index 6a6438e..115a872 100644 --- a/surface/src/components/Camera/Camera.scss +++ b/surface/src/components/Camera/Camera.scss @@ -15,25 +15,30 @@ .camera-row{ display: flex; flex-flow: row; - gap: 5px; + gap: 20px; justify-content: center; - align-items: center; overflow: hidden; } .frame{ border-radius: 10px; flex: 1 1 auto; - height: calc(50vh / 4); - width: 20% + background-size: cover; + background-position: center; + background-repeat: no-repeat; } .main-frame{ @extend .frame; height: calc(100vh / 2); + max-width: unset; + transition: ease 1s; } - +.main-frame-vert{ + z-index: 1; + transition: ease 1s; +} .frame:hover{ border: 2px solid cyan; diff --git a/surface/src/components/Camera/Camera.tsx b/surface/src/components/Camera/Camera.tsx index 479f90c..e7ee667 100644 --- a/surface/src/components/Camera/Camera.tsx +++ b/surface/src/components/Camera/Camera.tsx @@ -1,61 +1,32 @@ -/* eslint-env browser */ -import React from 'react'; +import * as React from 'react'; import CameraFrame from './CameraFrame'; import cameras from './camera.json'; import './Camera.scss'; -interface State{ - activeCamera: number -} - -export default class Camera extends React.Component<{}, State> { - constructor(props) { - super(props); - - this.state = { - activeCamera: 0, - }; - - this.handleClick = this.handleClick.bind(this); - this.handleKeyDown = this.handleKeyDown.bind(this); - } - - componentWillMount() { - document.addEventListener('keydown', this.handleKeyDown); - } +const Camera: React.FC = () => { + const [active, setActive] = React.useState(0); + const [rotate, setRotate] = React.useState(0); - componentWillUnmount() { - document.removeEventListener('keydown', this.handleKeyDown); - } - - handleKeyDown({ key }) { - key = parseInt(key); - if ([1,2,3,4].includes(key)) { - this.setState({ - activeCamera: key - 1 - }); - } - } - - handleClick(idx) { - this.setState({ - activeCamera: idx - 1 - }); - } - render() { - return( -
-
- -
-
- {cameras.map((camera, idx) => { - if (idx !== this.state.activeCamera) - return (); - })} -
+ return( +
+
+ {}} />
- ); - } - +
+ {cameras.map((camera, idx) => { + if (idx != active) + return ( { + setActive(idx); + setRotate(cameras[idx].angle); + }}/>); + })} + +
+
+ ) } + +export default Camera; \ No newline at end of file diff --git a/surface/src/components/Camera/CameraFrame.tsx b/surface/src/components/Camera/CameraFrame.tsx index 207ea7a..4aa6478 100644 --- a/surface/src/components/Camera/CameraFrame.tsx +++ b/surface/src/components/Camera/CameraFrame.tsx @@ -1,50 +1,46 @@ -import React from 'react'; +import * as React from 'react'; import CameraInterface from './CameraInterface'; import './Camera.scss'; interface Props{ handleClick(number): void - type: 'main' | 'secondary' + secondary?: boolean index?: number + rotate: number camera: CameraInterface } -interface State{ - img: string - loadImg: string - loaded: boolean -} - -export default class CameraFrame extends React.Component { - constructor(props) { - super(props); - - this.state={ - img: this.props.camera.feed, - loadImg: this.props.camera.placeholder, - loaded: false - }; - - this.imageError = this.imageError.bind(this); - this.imageLoad = this.imageLoad.bind(this); +const defaultProps: Props = { + handleClick: (_) => {}, + secondary: false, + rotate: 0, + camera: { + feed: '', + placeholder: '', + name: '', + angle: 0 } +} - imageError(){ - this.setState({img: this.props.camera.placeholder}); - } +const CameraFrame: React.FC = (props) => { + const [src, setSrc] = React.useState(props.camera.feed); + React.useEffect(() => { + setSrc(props.camera.feed); + }, [props.camera]) + + return( +
setSrc(props.camera.placeholder)} + onClick={() => props.handleClick(props.index)} + /> + ) +} - imageLoad(){ - this.setState({loaded: true}); - } +CameraFrame.defaultProps = defaultProps; - render() { - return ( - Image not found this.props.handleClick(this.props.index) : () => {}}/> - ); - } -} \ No newline at end of file +export default CameraFrame; \ No newline at end of file diff --git a/surface/src/components/Camera/CameraInterface.ts b/surface/src/components/Camera/CameraInterface.ts index a63faed..39e1d4c 100644 --- a/surface/src/components/Camera/CameraInterface.ts +++ b/surface/src/components/Camera/CameraInterface.ts @@ -2,4 +2,5 @@ export default interface CameraInterface{ feed: string placeholder: string name: string + angle: number } \ No newline at end of file diff --git a/surface/src/components/Camera/camera.json b/surface/src/components/Camera/camera.json index bc5b8f8..e075564 100644 --- a/surface/src/components/Camera/camera.json +++ b/surface/src/components/Camera/camera.json @@ -2,21 +2,37 @@ { "name": "Camera 1", "feed": "http://192.168.1.3:8090/cam0", - "placeholder": "./img/test_cam1.png" + "placeholder": "./img/test_cam1.png", + "angle": 0 }, { "name": "Camera 2", - "feed": "http://192.168.1.4:8090/cam1", - "placeholder": "./img/test_cam2.png" + "feed": "http://192.168.1.3:8090/cam1", + "placeholder": "./img/test_cam2.png", + "angle": 90 }, { "name": "Camera 3", - "feed": "http://192.168.1.2:8090/test.mjpg", - "placeholder": "./img/test_cam3.png" + "feed": "http://192.168.1.4:8090/cam0", + "placeholder": "./img/test_cam3.png", + "angle": 90 + }, + { + "name": "Camera 4", + "feed": "http://192.168.1.4:8090/cam1", + "placeholder": "./img/test_cam4.png", + "angle": 0 + }, + { + "name": "Camera 4", + "feed": "http://192.168.1.4:8090/cam2", + "placeholder": "./img/test_cam4.png", + "angle": 90 }, { "name": "Camera 4", - "feed": "./img/test_cam4.png", - "placeholder": "./img/test_cam4.png" + "feed": "http://192.168.1.4:8090/cam3", + "placeholder": "./img/test_cam4.png", + "angle": 270 } ] diff --git a/surface/src/components/IMU/IMU.tsx b/surface/src/components/IMU/IMU.tsx index dafe386..3b0eecb 100644 --- a/surface/src/components/IMU/IMU.tsx +++ b/surface/src/components/IMU/IMU.tsx @@ -55,6 +55,7 @@ const animate = (s: T) => { const IMU: React.FC = () => { const ref = React.useRef(null); + const [offsets, setOffsets] = React.useState>([0.0, 0.0, 0.0]); ipcRenderer.on('imu', (e, data) => { orientation = data; @@ -82,7 +83,14 @@ const IMU: React.FC = () => { }, []) return( -
+
+ +
+
) } diff --git a/surface/src/components/Log/Log.tsx b/surface/src/components/Log/Log.tsx index d4d33ec..f28eb6c 100644 --- a/surface/src/components/Log/Log.tsx +++ b/surface/src/components/Log/Log.tsx @@ -4,37 +4,30 @@ import {ipcRenderer} from 'electron'; import './Log.scss'; import * as channels from './channels'; -interface State{ - items: Array -} -export default class Log extends React.Component<{}, State> { - constructor(props){ - super(props); +const Log: React.FC = () => { + const [items, setItems] = React.useState>([]); - this.state = {items: []}; - } - componentDidMount(){ + React.useEffect(() => { ipcRenderer.send('logger', 'ready'); channels.default.map(channel => { ipcRenderer.on(channel, (e, data: LogItem) => { - this.addItem(data); + setItems(old => [...old, data]); }); }); - } - - addItem(item: LogItem){ - this.setState({items: [...this.state.items, item]}); - } + }, []) - render() { - return ( -
    - {this.state.items.map((item, index) => { - return
  1. {`${item.timestamp ? item.timestamp : ''} (${item.process}): ${item.text}`}
  2. - })} -
- ); - } + return( +
    + {items.map((item, index) => { + return
  1. + {`${item.timestamp ? item.timestamp : ''} (${item.process})`}: {item.text} +
  2. + })} +
+ ) } + +export default Log; + diff --git a/surface/src/components/Log/LogItem.ts b/surface/src/components/Log/LogItem.ts index 878124c..d4fac50 100644 --- a/surface/src/components/Log/LogItem.ts +++ b/surface/src/components/Log/LogItem.ts @@ -1,14 +1,21 @@ export interface LogItem{ timestamp: string process: string - text: string + text: string, + color?: string } -export default function(proc: string, text: string) { +export const LOG_NORMAL = '#f0ffff'; +export const LOG_WARNING = '#FFC900'; +export const LOG_ERROR = '#FF3600'; +export const LOG_SUCCESS = '#7AFF33'; + +export default (proc: string, text: string, color = LOG_NORMAL) => { let i: LogItem = { timestamp: new Date().toISOString().substr(14, 5), process: proc, - text: text + text: text, + color: color } return i; diff --git a/surface/src/components/Log/channels.ts b/surface/src/components/Log/channels.ts index 427f0c7..d80d32e 100644 --- a/surface/src/components/Log/channels.ts +++ b/surface/src/components/Log/channels.ts @@ -5,6 +5,10 @@ export const GAMEPAD = 'gamepad_listener'; export const SERVO = 'servo'; export const THRUSTERS = 'thruster_listener'; export const IMU = 'imu_listener'; +export const COM = 'com_sender'; +export const GENERAL = 'general'; +export const RAILCAP = 'rail_capture'; +export const RAILPROC = 'rail_process'; export default [ SET_IP, @@ -13,5 +17,9 @@ export default [ GAMEPAD, SERVO, THRUSTERS, - IMU + IMU, + COM, + GENERAL, + RAILCAP, + RAILPROC ] \ No newline at end of file diff --git a/surface/src/components/Mosaic/Mosaic.scss b/surface/src/components/Mosaic/Mosaic.scss new file mode 100644 index 0000000..f16a9c0 --- /dev/null +++ b/surface/src/components/Mosaic/Mosaic.scss @@ -0,0 +1,3 @@ +.mosaic-btn{ + +} \ No newline at end of file diff --git a/surface/src/components/Mosaic/Mosaic.tsx b/surface/src/components/Mosaic/Mosaic.tsx new file mode 100644 index 0000000..f48c324 --- /dev/null +++ b/surface/src/components/Mosaic/Mosaic.tsx @@ -0,0 +1,33 @@ +import { ipcRenderer } from 'electron'; +import * as React from 'react'; +import './Mosaic.scss'; + +const Mosaic: React.FC = () => { + const [counter, setCounter] = React.useState(0); + + React.useEffect(() => { + if(counter == 4){ + ipcRenderer.send('process_frames'); + } + }, [counter]) + + return( +
+
Mosaic Photo {counter}/5
+ +
+ ) +} + +export default Mosaic; \ No newline at end of file diff --git a/surface/src/components/Profiles/CoM/CoM.scss b/surface/src/components/Profiles/CoM/CoM.scss new file mode 100644 index 0000000..beb7209 --- /dev/null +++ b/surface/src/components/Profiles/CoM/CoM.scss @@ -0,0 +1,16 @@ +.com-container{ + display: grid; + grid-template-columns: 1fr; + grid-template-rows: auto repeat(3, 1fr); + gap: 5px; +} + +.com-amount{ + width: 100%; + text-align: center; +} + +.com-title{ + @extend .com-amount; + border-bottom: solid; +} \ No newline at end of file diff --git a/surface/src/components/Profiles/CoM/CoM.tsx b/surface/src/components/Profiles/CoM/CoM.tsx new file mode 100644 index 0000000..80a0fdc --- /dev/null +++ b/surface/src/components/Profiles/CoM/CoM.tsx @@ -0,0 +1,47 @@ +import { ipcRenderer } from 'electron'; +import * as React from 'react'; +import { GamepadParams } from '../../../../electron/gamepad'; +import Slider from '../../Slider/Slider'; +import './CoM.scss'; + +interface Props { + vals: Array +} + +const CoM: React.FC = (props) => { + const [values, setValues] = React.useState>([0.0, 0.0, 0.0]); + + React.useEffect(() => { + let temp = [...values]; + temp = props.vals.map((val, idx) => val); + setValues(temp); + + ipcRenderer.send('com_send', temp); + }, props.vals); + + return( +
+
CoM
+ {values.map((val, idx) => { + return( + { + let temp = [...values]; + temp[idx] = val; + setValues(temp); + + ipcRenderer.send('com_send', temp); + }} + /> + ) + })} +
+ ) +} + +export default CoM; \ No newline at end of file diff --git a/surface/src/components/Profiles/Compensator/Compensator.scss b/surface/src/components/Profiles/Compensator/Compensator.scss new file mode 100644 index 0000000..a8abe86 --- /dev/null +++ b/surface/src/components/Profiles/Compensator/Compensator.scss @@ -0,0 +1,16 @@ +.compensator-container{ + display: grid; + grid-template-columns: 1fr; + grid-template-rows: auto repeat(3, 1fr); + gap: 5px; +} + +.compensator-amount{ + width: 100%; + text-align: center; +} + +.compensator-title{ + @extend .compensator-amount; + border-bottom: solid; +} \ No newline at end of file diff --git a/surface/src/components/Profiles/Compensator/Compensator.tsx b/surface/src/components/Profiles/Compensator/Compensator.tsx new file mode 100644 index 0000000..3111d61 --- /dev/null +++ b/surface/src/components/Profiles/Compensator/Compensator.tsx @@ -0,0 +1,60 @@ +import { ipcRenderer } from 'electron'; +import { GamepadParams } from '../../../../electron/gamepad'; +import * as React from 'react'; +import Slider from '../../Slider/Slider'; +import './Compensator.scss'; + +interface Props { + vals: Array +} + +const Compensator: React.FC = (props) => { + const [values, setValues] = React.useState>([0.0, 0.0, 0.0]); + + React.useEffect(() => { + let temp = [...values]; + temp = props.vals.map((val, idx) => val); + setValues(temp); + + let params: GamepadParams = { + type: 'trim', + values: temp + } + + console.log(temp); + + ipcRenderer.send('compensator', params); + console.log('sent') + }, [props.vals]); + + return( +
+
Buoyancy
+ {values.map((val, idx) => { + return( + { + let temp = [...values]; + temp[idx] = val; + setValues(temp); + + let params: GamepadParams = { + type: 'trim', + values: temp + } + + ipcRenderer.send('compensator', params); + }} + /> + ) + })} +
+ ) +} + +export default Compensator; \ No newline at end of file diff --git a/surface/src/components/Profiles/Profiles.scss b/surface/src/components/Profiles/Profiles.scss new file mode 100644 index 0000000..bb45d83 --- /dev/null +++ b/surface/src/components/Profiles/Profiles.scss @@ -0,0 +1,8 @@ +.profile-container{ + display: flex; + flex-direction: column; +} + +.profile-container select{ + margin: 10px; +} \ No newline at end of file diff --git a/surface/src/components/Profiles/Profiles.tsx b/surface/src/components/Profiles/Profiles.tsx new file mode 100644 index 0000000..507a465 --- /dev/null +++ b/surface/src/components/Profiles/Profiles.tsx @@ -0,0 +1,41 @@ +import * as React from 'react'; +import './Profiles.scss'; +import configs from './configs.json'; +import ThrustTweaker from './Tweaker/ThrustTweaker'; +import Compensator from './Compensator/Compensator'; +import CoM from './CoM/CoM'; + +interface Config { + tweaker: Array + com: Array + compensator: Array +} + +const Profiles: React.FC = () => { + const [config, setConfig] = React.useState('test_config'); + + const updateConfig = (which) => setConfig(which); + + return( +
+ + + + + +
+ ) +} + +export default Profiles; \ No newline at end of file diff --git a/surface/src/components/Profiles/Tweaker/ThrustTweaker.scss b/surface/src/components/Profiles/Tweaker/ThrustTweaker.scss new file mode 100644 index 0000000..21f254e --- /dev/null +++ b/surface/src/components/Profiles/Tweaker/ThrustTweaker.scss @@ -0,0 +1,43 @@ +.tweaker-container{ + display: grid; + grid-template-columns: repeat(2, 1fr); + grid-template-rows: auto auto repeat(3, 1fr) auto auto; + grid-auto-flow: column; + gap: 5px; +} + +.tweaker-title{ + width: 100%; + text-align: center; + border-bottom: solid; + grid-column: 1 / 3; +} + +.tweaker-subtitle{ + @extend .tweaker-title; + grid-column: span 1; +} + +.tweaker-subtitle-left{ + @extend .tweaker-title; + grid-column: 1; + grid-row: 2; +} + +.tweaker-subtitle-right{ + @extend .tweaker-title; + grid-column: 2; + grid-row: 2; +} + +.reverse-container{ + grid-row: 6; + grid-column: 1 / 3; + display: flex; + justify-content: center; +} + +.fine-slider{ + grid-row: 7; + grid-column: 1/3; +} \ No newline at end of file diff --git a/surface/src/components/Profiles/Tweaker/ThrustTweaker.tsx b/surface/src/components/Profiles/Tweaker/ThrustTweaker.tsx new file mode 100644 index 0000000..f5626d6 --- /dev/null +++ b/surface/src/components/Profiles/Tweaker/ThrustTweaker.tsx @@ -0,0 +1,184 @@ +import { ipcRenderer } from 'electron'; +import * as React from 'react'; +import Slider from '../../Slider/Slider'; +import './ThrustTweaker.scss'; +import {GamepadParams} from '../../../../electron/gamepad'; +import Toggle from '../../Toggle/Toggle'; + +const TRANSLATION_MAX = 1.0; +const ROTATION_MAX = 1.0; +const TRANSLATION_STEP = 0.05; +const ROTATION_STEP = 0.05; + +interface Props { + vals: Array + fine: number + reverse: boolean + mode: boolean + lockout: boolean +} + +const ThrustTweaker: React.FC = (props) => { + const [values, setValues] = React.useState([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]); + const [fine, setFine] = React.useState(1.041) + const [reverse, setReverse] = React.useState(false); + const [mode, setMode] = React.useState(true); + const [lockout, setLockout] = React.useState(true); + + React.useEffect(() => { + setFine(props.fine); + + let params: GamepadParams = { + type: 'absolute', + values: [props.fine] + } + + ipcRenderer.send('absolute', params); + }, [props.fine]) + + React.useEffect(() => { + setReverse(props.reverse); + setMode(props.mode); + setLockout(props.lockout); + + let params: GamepadParams = { + type: 'reverse', + reverse: 'F' + } + + if(props.reverse) params.reverse = 'T'; + + ipcRenderer.send('reverse', params); + + params = { + type: 'mode', + mode: 'F' + } + + if(props.mode) params.mode = 'T'; + + ipcRenderer.send('mode', params); + + params = { + type: 'lockout', + lockout: 'F' + } + + if(props.lockout) params.lockout = 'T' + + ipcRenderer.send('lockout', params); + + }, [props.reverse, props.mode, props.lockout]) + + React.useEffect(() => { + let temp = [...values]; + temp = props.vals.map((val, idx) => val); + setValues(temp); + + let params: GamepadParams = { + type: 'scale', + values: temp + } + + ipcRenderer.send('gamepad_sock', params); + }, [props.vals]) + + const updateSwitch = () => { + let params: GamepadParams = { + type: 'reverse', + reverse: 'F' + } + + if(!reverse){ + params.reverse = 'T'; + } + + setReverse(!reverse); + + ipcRenderer.send('reverse', params); + } + + const updateMode = () => { + let params: GamepadParams = { + type: 'mode', + reverse: 'F' + } + + if(!mode){ + params.mode = 'T'; + } + + setMode(!mode); + + ipcRenderer.send('mode', params); + } + + const updateLockout = () => { + let params: GamepadParams = { + type: 'lockout', + lockout: 'F' + } + + if(!lockout){ + params.lockout = 'T'; + } + + setLockout(!lockout); + + ipcRenderer.send('lockout', params); + } + + return( +
+
Thrust Tweaker
+
Translation
+
Rotation
+ {values.map((v, idx) => { + return( + { + let temp = [...values]; + temp[idx] = val; + setValues(temp); + + let params: GamepadParams = { + type: 'scale', + values: values + } + + ipcRenderer.send('gamepad_sock', params); + } + }/> + ) + })} +
+ + + +
+
+ { + setFine(val); + + let params: GamepadParams = { + type: 'absolute', + values: [val] + } + + ipcRenderer.send('absolute', params); + }}/> +
+
+ ) +} + +export default ThrustTweaker; \ No newline at end of file diff --git a/surface/src/components/Profiles/configs.json b/surface/src/components/Profiles/configs.json new file mode 100644 index 0000000..7be16d9 --- /dev/null +++ b/surface/src/components/Profiles/configs.json @@ -0,0 +1,52 @@ +{ + "test_config":{ + "name": "Test Config", + "tweaker":[1.0, 0.1, 0.5, 1.0, 1.0, 1.0], + "fine": 1.041, + "mode": true, + "reverse": false, + "lockout": true, + "com": [0, 0, -0.2], + "trim": [0, 0, 0] + }, + "other_config":{ + "name": "Other Config", + "tweaker":[0.0, 0.1, 0.2, 1.0, 0.8, 1.0], + "fine": 1.041, + "mode": true, + "reverse": true, + "lockout": true, + "com": [0, 0, -0.2], + "trim": [0, 0, 1] + }, + "seabin":{ + "name": "Seabin Config", + "tweaker":[0.2, 0.5, 0.2, 1.0, 0.2, 0.2], + "fine": 1.041, + "mode": true, + "reverse": false, + "lockout": false, + "com": [0, 0, -0.2], + "trim": [0, 0, -1] + }, + "ghost":{ + "name": "Ghost Net Config", + "tweaker":[0.5, 0.2, 0.3, 1.0, 0.2, 0.2], + "fine": 1.041, + "mode": true, + "reverse": false, + "lockout": true, + "com": [0, 0, -0.2], + "trim": [0, 0, -1] + }, + "sponge":{ + "name": "Sponge Config", + "tweaker":[0.2, 0.5, 0.2, 1.0, 0.2, 0.2], + "fine": 1.041, + "mode": true, + "reverse": true, + "lockout": false, + "com": [0, 0, -0.2], + "trim": [0, 0, -2] + } +} \ No newline at end of file diff --git a/surface/src/components/Servo/Servo.scss b/surface/src/components/Servo/Servo.scss index 36b33fd..cb251f0 100644 --- a/surface/src/components/Servo/Servo.scss +++ b/surface/src/components/Servo/Servo.scss @@ -1,7 +1,7 @@ .servo-container{ display: grid; grid-template-columns: 1fr; - grid-template-rows: repeat(3, 1fr); + grid-template-rows: repeat(3, auto); gap: 5px; } diff --git a/surface/src/components/Servo/Servo.tsx b/surface/src/components/Servo/Servo.tsx index fd048fa..12ab420 100644 --- a/surface/src/components/Servo/Servo.tsx +++ b/surface/src/components/Servo/Servo.tsx @@ -3,33 +3,22 @@ import * as React from 'react'; import Slider from '../Slider/Slider'; import './Servo.scss'; -interface State{ - value: number -} - -export default class Servo extends React.Component<{}, State>{ - constructor(props){ - super(props); - - this.state = { - value: 50 - } +const Servo: React.FC = () => { + const [value, setValue] = React.useState(50); - this.updateValue = this.updateValue.bind(this); - } - - updateValue(value){ - this.setState({value: value}); - ipcRenderer.send('servo_send', value); - } + return( +
+
Camera Angle
+ { + setValue(val); + ipcRenderer.send('servo_send', val); + }} + /> +
{value}°
+
+ ) +} - render(){ - return( -
-
Camera Angle
- -
{this.state.value}°
-
- ) - } -} \ No newline at end of file +export default Servo; \ No newline at end of file diff --git a/surface/src/components/Slider/Slider.scss b/surface/src/components/Slider/Slider.scss index c88b18e..1a3328e 100644 --- a/surface/src/components/Slider/Slider.scss +++ b/surface/src/components/Slider/Slider.scss @@ -1,9 +1,32 @@ +.slider-container{ + display: grid; + grid-template-columns: 1fr; + grid-template-rows: 1fr 1fr; + gap: 5px; +} + +.slider-range{ + margin: auto; +} + +.slider-top{ + display: flex; + gap: 5px; +} + +.slider-bottom{ + display: grid; + grid-template-columns: repeat(3, 1fr); + height: 20px; +} + .slider{ appearance: none; width: 100%; height: 10px; background: #d3d3d3; outline: none; + margin: auto; opacity: 0.7; transition: opacity .2; border-radius: 10px; @@ -20,4 +43,20 @@ border-radius: 10px; background: #00eeff; cursor: pointer; +} + +.zero-btn{ + background-color: steelblue; + border: none; + border-radius: 5px; + width: 20px; + height: 100%; +} + +.inputbox { + border-radius: 5px; + border: none; + //width: 60px; + align-items: center; + text-align: center; } \ No newline at end of file diff --git a/surface/src/components/Slider/Slider.tsx b/surface/src/components/Slider/Slider.tsx index 973ddb5..2d84ea6 100644 --- a/surface/src/components/Slider/Slider.tsx +++ b/surface/src/components/Slider/Slider.tsx @@ -1,51 +1,147 @@ import * as React from 'react'; import './Slider.scss'; +//FULL SLIDER COMPONENT PROPS interface Props{ - vertical: boolean - min: number - max: number + //vertical?: boolean + min?: number + max?: number + step?: number + unit?: string callback(val: number): void + value: number +} + +const defaultProps: Props = { + //vertical: false, + min: 0, + max: 100, + step: 1, + callback: (_) => {}, + value: 0, + unit: '' } -interface State{ +//SLIDER BAR PROPS +interface sliderProps{ + min?: number + max?: number + step?: number + callback(val: React.ChangeEvent): void value: number } -export default class Slider extends React.Component { - static defaultProps = { - vertical: false, - min: 0, - max: 100, - initVal: 50, - callback: () => {} - } +//BOTTOM DISPLAY PROPS +interface displayProps{ + value: number + unit? : string + callback(val: boolean): void +} - constructor(props) { - super(props); +//BOTTOM INPUT BOX PROPS +interface inputProps{ + value: number + min?: number + max?: number + step?: number + callback(val: React.FocusEvent): void +} - this.state = { - value: 50 - } +const SliderBar: React.FC = ({value, min, max, step, callback}) => { + return( +
+ {min} + callback(e)} + /> + {max} +
+ ) +} - this.handleChange = this.handleChange.bind(this); - } +const ValueDisplay: React.FC = ({value, callback, unit}) => { + return( + {callback(true)}}>{value}{unit} + + ) +} + +const Inputbox: React.FC = ({value, max, min, callback, step}) => { + return( + e.target.select()} + onKeyUp={(e) => { + if (e.key === 'Enter') { + const target = e.target as HTMLInputElement; + target.blur(); + } + }} + onBlur={(e) => callback(e)} + > + ) +} - handleChange(val) { - this.setState({value: val.target.value}); - this.props.callback(this.state.value); - } - render() { - return ( - - ) - } +const Slider: React.FC = (props) => { + const [starting, setStarting] = React.useState(0); + const [inputActive, setInputActive] = React.useState(false); + React.useEffect(() => { + setStarting(Object.assign({}, props).value); + }, []) + + return ( +
+ { + const target = e.target as HTMLInputElement + props.callback(parseFloat(target.value)) + }}> +
+ + {inputActive ? ( + { + let target = e.target as HTMLInputElement + let changeto = parseFloat(target.value); + if(isNaN(changeto) || changeto === Infinity) { + changeto = props.value!; + } + if(changeto > props.max!) { + changeto = props.max!; + } else if(changeto < props.min!) { + changeto = props.min!; + } + props.callback(changeto); + setInputActive(false); + }}> + ) : ( + { + setInputActive(bool); + }}> + )} +
+
+ + ) } + +Slider.defaultProps = defaultProps; + +export default Slider; \ No newline at end of file diff --git a/surface/src/components/Switch/Switch.scss b/surface/src/components/Switch/Switch.scss new file mode 100644 index 0000000..ef88ff4 --- /dev/null +++ b/surface/src/components/Switch/Switch.scss @@ -0,0 +1,70 @@ +$active: #00eeff; +$active-inner: #ffffff; +$focus: 2px rgba(39, 94, 254, 0.302); +$border: #BBC1E1; +$border-hover: #00eeff; +$background: $active-inner; + +$b: $background; +$bc: $border; +$d-t: .3s; +$d-t-e: ease; +$d-o: .2s; + +$ab: $border; + +.switch-container{ + display: flex; + flex-flow: row; + justify-content: center; +} + +.switch{ + -webkit-appearance: none; + height: 50px; + width: 150px; + outline: none; + display: inline-block; + vertical-align: top; + position: relative; + margin: 0; + cursor: pointer; + border: 1px solid $border; + border-radius: 25px; + background: $b; + transition: background .3s, border-color .3s, box-shadow .2s; +} + +.switch::after{ + content: ''; + display: block; + position: absolute; + transition: transform .3s ease, opacity .2s; + left: 9px; + top: 4px; + border-radius: 50%; + width: 40px; + height: 40px; + background: $ab; + transform: translateX(0px); +} + +.switch:hover{ + border: 1px solid $border-hover; +} + +.switch:checked{ + background: $active; + border: 1px solid $active; + &::after{ + background: $active-inner; + transform: translateX(90px); + transition: transform .6s cubic-bezier(.2, .85, .32, 1.2), opacity .3s; + } +} + +.switch:not(:checked){ + &::after{ + opacity: .6; + } +} \ No newline at end of file diff --git a/surface/src/components/Switch/Switch.tsx b/surface/src/components/Switch/Switch.tsx new file mode 100644 index 0000000..f857151 --- /dev/null +++ b/surface/src/components/Switch/Switch.tsx @@ -0,0 +1,19 @@ +import * as React from 'react'; +import './Switch.scss'; + +interface Props { + checked: boolean + callback(): void +} + +const Switch: React.FC = (props) => { + + return( +
+ +
+ + ) +} + +export default Switch; \ No newline at end of file diff --git a/surface/src/components/Thrusters/ThrusterCircle/ThrusterCircle.tsx b/surface/src/components/Thrusters/ThrusterCircle/ThrusterCircle.tsx index 93b5da5..6895d4b 100644 --- a/surface/src/components/Thrusters/ThrusterCircle/ThrusterCircle.tsx +++ b/surface/src/components/Thrusters/ThrusterCircle/ThrusterCircle.tsx @@ -7,56 +7,42 @@ interface Props{ name: string } -export default class ThrusterCircle extends React.Component { - circleStyle: {} - - constructor(props: Props) { - super(props); - - this.circleStyle = { - backgroundImage: 'linear-gradient(91deg, transparent 50%, #A6A6A6 50%), linear-gradient(90deg, #A6A6A6 50%, transparent 50%)' - }; - - this.setValue = this.setValue.bind(this); - this.setValue(); - - } - - componentDidUpdate(){ - this.setValue(); - } - - setValue() { - var color = '#39B4CC'; - if(this.props.thrust < 127){ - color = '#FF4747'; - } - var output = Math.round((Math.abs(this.props.thrust - 127) / 127) * 360); +const l180 = (val: number, original: number) => { + return({ + backgroundImage: + `linear-gradient(${val + 90}deg, transparent 50%, #A6A6A6 50%),` + + `linear-gradient(90deg, #A6A6A6 50%, transparent 50%)`, + backgroundColor: original < 127 ? '#FF4747' : '#39B4CC' + }) +} - if(output <= 180){ - this.circleStyle = { - backgroundImage: 'linear-gradient(' + (output + 90) +'deg, transparent 50%, #A6A6A6 50%),linear-gradient(90deg, #A6A6A6 50%, transparent 50%)', - backgroundColor: color - }; - }else{ - this.circleStyle = { - backgroundImage: 'linear-gradient(' + (output - 90) +'deg, transparent 50%, ' + color + ' 50%),linear-gradient(90deg, #A6A6A6 50%, transparent 50%)', - backgroundColor: color - }; - } - } +const g180 = (val: number, original: number) => { + return({ + backgroundImage: + `linear-gradient(${val - 90}deg, transparent 50%, ${original < 127 ? '#FF4747' : '#39B4CC'} 50%),` + + `linear-gradient(90deg, #A6A6A6 50%, transparent 50%)`, + backgroundColor: original < 127 ? '#FF4747' : '#39B4CC' + }) +} - render() { - return ( -
-
-
- {Math.round(((Math.abs(this.props.thrust) - 127) / 127) * 100)}% -
- {this.props.name} -
+const ThrusterCircle: React.FC = (props) => { + const [angle, setAngle] = React.useState(Math.round(((Math.abs(props.thrust) - 127) / 127) * 360)); + + React.useEffect(() => { + setAngle(Math.round(((Math.abs(props.thrust) - 127) / 127) * 360)) + }, [props.thrust]); + + return( +
+
+
+ {Math.round(angle / 360 * 100)}% +
+ {props.name}
-
- ); - } +
+
+ ) } + +export default ThrusterCircle; \ No newline at end of file diff --git a/surface/src/components/Thrusters/ThrusterInfo/ThrusterInfo.tsx b/surface/src/components/Thrusters/ThrusterInfo/ThrusterInfo.tsx index e91937c..de532fd 100644 --- a/surface/src/components/Thrusters/ThrusterInfo/ThrusterInfo.tsx +++ b/surface/src/components/Thrusters/ThrusterInfo/ThrusterInfo.tsx @@ -5,35 +5,25 @@ import './ThrusterInfo.scss'; const INIT_THRUST = 127; -interface State{ - thrust: Array -} - -export default class ThrusterInfo extends React.Component<{}, State> { - constructor(props: {}) { - super(props); +const ThrusterInfo: React.FC = () => { + const [thrust, setThrust] = React.useState>(Array(8).fill(INIT_THRUST)); - this.state = {thrust: [INIT_THRUST, INIT_THRUST, INIT_THRUST, INIT_THRUST, INIT_THRUST, INIT_THRUST, INIT_THRUST, INIT_THRUST]}; - } + ipcRenderer.on('thrusters', (e, data: Array) => { + setThrust(data) + }); - componentDidMount(){ - ipcRenderer.on('thrusters', (e, data: Array) => { - this.setState({thrust: data}) - }); - } - - render() { - return ( -
- - - - - - - - -
- ); - } + return ( +
+ + + + + + + + +
+ ) } + +export default ThrusterInfo; diff --git a/surface/src/components/Thrusters/Tweaker/ThrustTweaker.scss b/surface/src/components/Thrusters/Tweaker/ThrustTweaker.scss deleted file mode 100644 index 446c097..0000000 --- a/surface/src/components/Thrusters/Tweaker/ThrustTweaker.scss +++ /dev/null @@ -1,18 +0,0 @@ -.tweaker-container{ - display: grid; - grid-template-columns: repeat(2, 1fr); - grid-template-rows: repeat(5, 1fr); - gap: 5px; -} - -.tweaker-title{ - width: 100%; - text-align: center; - border-bottom: solid; - grid-column: 1 / 3; -} - -.tweaker-subtitle{ - @extend .tweaker-title; - grid-column: span 1; -} \ No newline at end of file diff --git a/surface/src/components/Thrusters/Tweaker/ThrustTweaker.tsx b/surface/src/components/Thrusters/Tweaker/ThrustTweaker.tsx deleted file mode 100644 index bc664b8..0000000 --- a/surface/src/components/Thrusters/Tweaker/ThrustTweaker.tsx +++ /dev/null @@ -1,40 +0,0 @@ -import { ipcRenderer } from 'electron'; -import * as React from 'react'; -import Slider from '../../Slider/Slider'; -import './ThrustTweaker.scss'; - -interface State{ - values: Array -} - -export default class Servo extends React.Component<{}, State>{ - constructor(props){ - super(props); - - this.state = { - values: [0, 0, 0, 0, 0, 0] - } - - this.updateValue = this.updateValue.bind(this); - } - - updateValue(value){ - - } - - render(){ - return( -
-
Thrust Tweaker
-
Translation
-
Rotation
- - - - - - -
- ) - } -} \ No newline at end of file diff --git a/surface/src/components/Toggle/Toggle.scss b/surface/src/components/Toggle/Toggle.scss new file mode 100644 index 0000000..a387bc1 --- /dev/null +++ b/surface/src/components/Toggle/Toggle.scss @@ -0,0 +1,21 @@ +.toggle-base{ + border: 2px solid #2e2e2e; + border-radius: 10px; + &:focus{ + outline: none; + } + &:hover{ + border: 2px solid cyan; + } + height: 50px; +} + +.toggle-true{ + @extend .toggle-base; + background-color: green; +} + +.toggle-false{ + @extend .toggle-base; + background-color: red; +} \ No newline at end of file diff --git a/surface/src/components/Toggle/Toggle.tsx b/surface/src/components/Toggle/Toggle.tsx new file mode 100644 index 0000000..5c1246a --- /dev/null +++ b/surface/src/components/Toggle/Toggle.tsx @@ -0,0 +1,17 @@ +import * as React from 'react'; +import './Toggle.scss'; + +interface Props { + name: string + toggled: boolean + callback(): void +} + +const Toggle: React.FC = (props) => { + + return( + + ) +} + +export default Toggle; \ No newline at end of file diff --git a/surface/src/main.scss b/surface/src/main.scss index 5c6161a..8c8d081 100644 --- a/surface/src/main.scss +++ b/surface/src/main.scss @@ -31,7 +31,7 @@ html{ body{ background-color: $color-bg; - color: azure; + color: #f0ffff; margin: 0; height: 100vh; display: flex; diff --git a/surface/webpack.config.js b/surface/webpack.config.js index dd62293..da7fb4f 100644 --- a/surface/webpack.config.js +++ b/surface/webpack.config.js @@ -3,7 +3,6 @@ const path = require('path'); module.exports = [ { - mode: 'development', entry: './main.ts', target: 'electron-main', module: { @@ -27,7 +26,6 @@ module.exports = [ } }, { - mode: 'development', entry: './src/index.tsx', target: 'electron-renderer', devtool: 'source-map',