diff --git a/auv_setup/config/robots/beluga.yaml b/auv_setup/config/robots/beluga.yaml
index 9e6da6fad..0855fed65 100644
--- a/auv_setup/config/robots/beluga.yaml
+++ b/auv_setup/config/robots/beluga.yaml
@@ -188,7 +188,7 @@ temperature:
i2c:
pca9685:
address: 0x40
- bus: 8
+ bus: 1
psm:
address_voltage: 0x6a
address_current: 0x69
diff --git a/auv_setup/launch/beluga.launch b/auv_setup/launch/beluga.launch
index 425ff54ad..a1b6a354d 100644
--- a/auv_setup/launch/beluga.launch
+++ b/auv_setup/launch/beluga.launch
@@ -5,7 +5,8 @@
-
+
+
@@ -20,14 +21,16 @@
-
+
-
+
+
+
diff --git a/manipulators/gripper_interface/scripts/gripper_interface_node.py b/manipulators/gripper_interface/scripts/gripper_interface_node.py
index 0e5b80bfc..3f3b5e801 100755
--- a/manipulators/gripper_interface/scripts/gripper_interface_node.py
+++ b/manipulators/gripper_interface/scripts/gripper_interface_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from sensor_msgs.msg import Joy
diff --git a/manipulators/lights_interface/scripts/lights_interface_node.py b/manipulators/lights_interface/scripts/lights_interface_node.py
index 0b54295af..6dd04ef23 100755
--- a/manipulators/lights_interface/scripts/lights_interface_node.py
+++ b/manipulators/lights_interface/scripts/lights_interface_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from sensor_msgs.msg import Joy
diff --git a/manipulators/torpedo/scripts/torpedo_launch_service.py b/manipulators/torpedo/scripts/torpedo_launch_service.py
index a6e6dd9de..315d5b801 100755
--- a/manipulators/torpedo/scripts/torpedo_launch_service.py
+++ b/manipulators/torpedo/scripts/torpedo_launch_service.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# coding: UTF-8
import rospy
@@ -47,4 +47,4 @@ def execute(self, req):
if __name__ == "__main__":
node = TorpedoLaunch()
while not rospy.is_shutdown():
- rospy.spin()
\ No newline at end of file
+ rospy.spin()
diff --git a/mission/anomaly_detection/cfg/BatterySimulator.cfg b/mission/anomaly_detection/cfg/BatterySimulator.cfg
index 61fe4d7a8..7853790e7 100755
--- a/mission/anomaly_detection/cfg/BatterySimulator.cfg
+++ b/mission/anomaly_detection/cfg/BatterySimulator.cfg
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
PACKAGE = "anomaly_detection"
diff --git a/mission/anomaly_detection/scripts/battery_simulator.py b/mission/anomaly_detection/scripts/battery_simulator.py
index abf49cf04..7df0a4b28 100755
--- a/mission/anomaly_detection/scripts/battery_simulator.py
+++ b/mission/anomaly_detection/scripts/battery_simulator.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
diff --git a/mission/finite_state_machine/scripts/four_corner_mission.py b/mission/finite_state_machine/scripts/four_corner_mission.py
index 68e30c156..d2280feb9 100755
--- a/mission/finite_state_machine/scripts/four_corner_mission.py
+++ b/mission/finite_state_machine/scripts/four_corner_mission.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from smach import Sequence
diff --git a/mission/finite_state_machine/scripts/fsm_helper.py b/mission/finite_state_machine/scripts/fsm_helper.py
index 765c8bb1b..32bf65fae 100755
--- a/mission/finite_state_machine/scripts/fsm_helper.py
+++ b/mission/finite_state_machine/scripts/fsm_helper.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# coding: UTF-8
from enum import IntEnum
diff --git a/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py b/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py
index 0b6b8eb60..f10378591 100755
--- a/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py
+++ b/mission/finite_state_machine/scripts/go_to_and_inspect_pt_sm.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
import numpy as np
diff --git a/mission/finite_state_machine/scripts/goal_pose_server.py b/mission/finite_state_machine/scripts/goal_pose_server.py
index d1421b1f6..8ce48702f 100755
--- a/mission/finite_state_machine/scripts/goal_pose_server.py
+++ b/mission/finite_state_machine/scripts/goal_pose_server.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from geometry_msgs.msg import Point
diff --git a/mission/finite_state_machine/scripts/perception_simulator.py b/mission/finite_state_machine/scripts/perception_simulator.py
index 29355b416..dd3c66e40 100755
--- a/mission/finite_state_machine/scripts/perception_simulator.py
+++ b/mission/finite_state_machine/scripts/perception_simulator.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from std_msgs.msg import String
diff --git a/mission/finite_state_machine/scripts/pooltest.py b/mission/finite_state_machine/scripts/pooltest.py
index 6da674188..89f001b67 100755
--- a/mission/finite_state_machine/scripts/pooltest.py
+++ b/mission/finite_state_machine/scripts/pooltest.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# coding: UTF-8
from math import pi
diff --git a/mission/finite_state_machine/scripts/prequalification_fsm.py b/mission/finite_state_machine/scripts/prequalification_fsm.py
index bdb9c87a9..8eb30bcc0 100755
--- a/mission/finite_state_machine/scripts/prequalification_fsm.py
+++ b/mission/finite_state_machine/scripts/prequalification_fsm.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from smach import StateMachine
diff --git a/mission/finite_state_machine/scripts/robosub_fsm.py b/mission/finite_state_machine/scripts/robosub_fsm.py
index 03da4aed0..abdbb11ec 100755
--- a/mission/finite_state_machine/scripts/robosub_fsm.py
+++ b/mission/finite_state_machine/scripts/robosub_fsm.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from smach import StateMachine
diff --git a/mission/finite_state_machine/scripts/simtest.py b/mission/finite_state_machine/scripts/simtest.py
index 5d682a537..d955685a3 100755
--- a/mission/finite_state_machine/scripts/simtest.py
+++ b/mission/finite_state_machine/scripts/simtest.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# coding: UTF-8
import rospy
diff --git a/mission/finite_state_machine/scripts/simulator_state_machine.py b/mission/finite_state_machine/scripts/simulator_state_machine.py
index 165c1a6e5..67c7e5d7d 100755
--- a/mission/finite_state_machine/scripts/simulator_state_machine.py
+++ b/mission/finite_state_machine/scripts/simulator_state_machine.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from smach import StateMachine
diff --git a/mission/finite_state_machine/setup.py b/mission/finite_state_machine/setup.py
index f1701e909..903ded0e6 100644
--- a/mission/finite_state_machine/setup.py
+++ b/mission/finite_state_machine/setup.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
diff --git a/mission/finite_state_machine/src/finite_state_machine/common_states.py b/mission/finite_state_machine/src/finite_state_machine/common_states.py
index 0d598294a..295995f2f 100644
--- a/mission/finite_state_machine/src/finite_state_machine/common_states.py
+++ b/mission/finite_state_machine/src/finite_state_machine/common_states.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
from enum import IntEnum
diff --git a/mission/finite_state_machine/src/finite_state_machine/go_home.py b/mission/finite_state_machine/src/finite_state_machine/go_home.py
index 91dfd32bc..29f887c8d 100755
--- a/mission/finite_state_machine/src/finite_state_machine/go_home.py
+++ b/mission/finite_state_machine/src/finite_state_machine/go_home.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
from math import pi
diff --git a/mission/finite_state_machine/src/finite_state_machine/helper.py b/mission/finite_state_machine/src/finite_state_machine/helper.py
index 20c725cb9..106e4c88d 100644
--- a/mission/finite_state_machine/src/finite_state_machine/helper.py
+++ b/mission/finite_state_machine/src/finite_state_machine/helper.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import math
from enum import IntEnum
diff --git a/mission/finite_state_machine/src/finite_state_machine/system_identification.py b/mission/finite_state_machine/src/finite_state_machine/system_identification.py
index d5469a015..7ca9b9621 100755
--- a/mission/finite_state_machine/src/finite_state_machine/system_identification.py
+++ b/mission/finite_state_machine/src/finite_state_machine/system_identification.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from smach import State
diff --git a/mission/internal_status/src/battery_monitor.py b/mission/internal_status/src/battery_monitor.py
index d219f7f07..8ee0bdf6e 100755
--- a/mission/internal_status/src/battery_monitor.py
+++ b/mission/internal_status/src/battery_monitor.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import subprocess
import time
@@ -10,173 +10,172 @@
class BatteryMonitor:
- def __init__(self):
-
- rospy.init_node("battery_monitor")
-
- # Parameters
- self.path_to_xavier_measurement = rospy.get_param("/battery/xavier/path", default="/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input")
- self.path_to_powersense = rospy.get_param("/battery/system/path", default="/dev/i2c-8")
-
- self.i2c_address_powersense_voltage = rospy.get_param("/i2c/psm/address_voltage", default=0x6a)
- self.i2c_address_powersense_current = rospy.get_param("/i2c/psm/address_current", default=0x69)
- self.i2c_bus_number = rospy.get_param("/i2c/psm/bus_number", default=8)
- rospy.loginfo("PSM Voltage I2C address: '0x{:02x}'".format(self.i2c_address_powersense_voltage))
- rospy.loginfo("PSM Current I2C address: '0x{:02x}'".format(self.i2c_address_powersense_current))
- rospy.loginfo("PSM I2C bus number: '%d'", self.i2c_bus_number)
-
- self.critical_level = rospy.get_param("/battery/thresholds/critical", default=13.5)
- self.warning_level = rospy.get_param("/battery/thresholds/warning", default=14.5)
-
- system_interval = rospy.get_param("/battery/system/interval", 0.05)
- xavier_interval = rospy.get_param("/battery/xavier/interval", 10)
- logging_interval = rospy.get_param("/battery/logging/interval", 10)
-
- # Local variables
- self.xavier_voltage = 0.0
- self.system_voltage = 0.0
- self.system_current = 0.0
-
- self.xavier_recieved = False
- self.system_recieved = False
+ def __init__(self):
+
+ rospy.init_node("battery_monitor")
+
+ # Parameters
+ self.path_to_xavier_measurement = rospy.get_param("/battery/xavier/path", default="/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input")
+ self.path_to_powersense = rospy.get_param("/battery/system/path", default="/dev/i2c-8")
+
+ self.i2c_address_powersense_voltage = rospy.get_param("/i2c/psm/address_voltage", default=0x6a)
+ self.i2c_address_powersense_current = rospy.get_param("/i2c/psm/address_current", default=0x69)
+ self.i2c_bus_number = rospy.get_param("/i2c/psm/bus_number", default=8)
+ rospy.loginfo("PSM Voltage I2C address: '0x{:02x}'".format(self.i2c_address_powersense_voltage))
+ rospy.loginfo("PSM Current I2C address: '0x{:02x}'".format(self.i2c_address_powersense_current))
+ rospy.loginfo("PSM I2C bus number: '%d'", self.i2c_bus_number)
+
+ self.critical_level = rospy.get_param("/battery/thresholds/critical", default=13.5)
+ self.warning_level = rospy.get_param("/battery/thresholds/warning", default=14.5)
+
+ system_interval = rospy.get_param("/battery/system/interval", 0.05)
+ xavier_interval = rospy.get_param("/battery/xavier/interval", 10)
+ logging_interval = rospy.get_param("/battery/logging/interval", 10)
+
+ # Local variables
+ self.xavier_voltage = 0.0
+ self.system_voltage = 0.0
+ self.system_current = 0.0
+
+ self.xavier_recieved = False
+ self.system_recieved = False
- self.is_PSM_fuckd_voltage = False
- self.is_PSM_fuckd_current = False
-
+ self.is_PSM_fuckd_voltage = False
+ self.is_PSM_fuckd_current = False
+
# Get I2C bus for power sense module
- self.bus = smbus.SMBus(self.i2c_bus_number)
- time.sleep(1)
-
- # Send configure command to the module to enable continuous conversion in 12-bit mode
- try:
- self.bus.write_byte(self.i2c_address_powersense_voltage, 0x10)
- time.sleep(0.5)
- except IOError:
- print(traceback.format_exc())
- self.is_PSM_fuckd_voltage = True
-
- try:
- self.bus.write_byte(self.i2c_address_powersense_current, 0x10)
- time.sleep(0.5)
- except IOError:
- print(traceback.format_exc())
- self.is_PSM_fuckd_current = True
-
-
- # Publishers
- self.xavier_battery_level_pub = rospy.Publisher(
- "/auv/battery_level/xavier", Float32, queue_size=1
- )
- if not self.is_PSM_fuckd_voltage:
- self.system_battery_level_pub = rospy.Publisher(
- "/auv/battery_level/system", Float32, queue_size=1
- )
- if not self.is_PSM_fuckd_current:
- self.system_battery_current_draw_pub = rospy.Publisher(
- "/auv/battery_level/system_current_draw", Float32, queue_size=1
- )
-
- # set up callbacks
-
- self.xavier_timer = rospy.Timer(rospy.Duration(secs=xavier_interval), self.xavier_cb)
- self.log_timer = rospy.Timer(rospy.Duration(secs=logging_interval), self.log_cb)
-
- if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current:
- self.system_timer = rospy.Timer(rospy.Duration(secs=system_interval), self.system_cb)
- rospy.loginfo("BatteryMonitor initialized")
- else:
- rospy.logwarn("System Battery Monitoring from PSM is not available.")
-
- def xavier_cb(self, event):
- """Record output from voltage meter command, decode from bytes object to string, convert from string to integer"""
- xavier_mV = int(
- subprocess.check_output(["cat", self.path_to_xavier_measurement]).decode("utf-8")
- )
- self.xavier_voltage = xavier_mV / 1000.0
-
- self.xavier_battery_level_pub.publish(self.xavier_voltage)
- if not self.xavier_recieved:
- self.xavier_recieved = True
-
- def system_cb(self, event):
- """Read voltage of system from powersense device."""
-
- # MCP3425 address, 0x68(104)
- # Read data back from 0x00(00), 2 bytes, MSB first
- # raw_adc MSB, raw_adc LSB
- if not self.is_PSM_fuckd_voltage:
- voltage_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_voltage, 0x00, 2)
-
- # Convert the data to 12-bits
- raw_adc_voltage = (voltage_msg[0] & 0x0F) * 256 + voltage_msg[1]
- if raw_adc_voltage > 2047:
- raw_adc_voltage -= 4095
-
- # MCP3425 address, 0x68(104)
- # Read data back from 0x00(00), 2 bytes, MSB first
- # raw_adc MSB, raw_adc LSB
- if not self.is_PSM_fuckd_current:
- current_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_current, 0x00, 2)
-
- # Convert the data to 12-bits
- raw_adc_current = (current_msg[0] & 0x0F) * 256 + current_msg[1]
- if raw_adc_current > 2047:
- raw_adc_current -= 4095
-
- # PSM specific conversion ratio
- if not self.is_PSM_fuckd_voltage:
- self.system_voltage = raw_adc_voltage * 0.011
- if not self.is_PSM_fuckd_current:
- self.system_current = raw_adc_current * 0.0504
-
- # publish
- if not self.is_PSM_fuckd_voltage:
- self.system_battery_level_pub.publish(self.system_voltage)
- if not self.is_PSM_fuckd_current:
- self.system_battery_current_draw_pub.publish(self.system_current)
- if not self.system_recieved:
- self.system_recieved = True
-
- def log_cb(self, event):
-
- if self.xavier_recieved:
- self.log_voltage(self.xavier_voltage, "xavier")
- else:
- rospy.loginfo("No voltage recieved from Xavier yet.")
-
- if self.system_recieved:
- self.log_voltage(self.system_voltage, "system")
- else:
- rospy.loginfo("No voltage recieved from system yet.")
-
-
- def log_voltage(self, voltage, title):
-
- if voltage == 0:
- rospy.loginfo("Voltage is zero. Killswitch is probably off.")
-
- # Critical voltage level
- elif voltage <= self.critical_level:
- rospy.logerr("Critical %s voltage: %.3fV" % (title, voltage))
-
- # Warning voltage level
- elif voltage <= self.warning_level:
- rospy.logwarn("%s voltage: %.3fV" % (title, voltage))
-
- else:
- rospy.loginfo("%s voltage: %.3fV" % (title, voltage))
-
- def shutdown(self):
- if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current:
- self.system_timer.shutdown()
- self.xavier_timer.shutdown()
- self.log_timer.shutdown()
- self.bus.close()
+ self.bus = smbus.SMBus(self.i2c_bus_number)
+ time.sleep(1)
+
+ # Send configure command to the module to enable continuous conversion in 12-bit mode
+ try:
+ self.bus.write_byte(self.i2c_address_powersense_voltage, 0x10)
+ time.sleep(0.5)
+ except IOError:
+ print(traceback.format_exc())
+ self.is_PSM_fuckd_voltage = True
+
+ try:
+ self.bus.write_byte(self.i2c_address_powersense_current, 0x10)
+ time.sleep(0.5)
+ except IOError:
+ print(traceback.format_exc())
+ self.is_PSM_fuckd_current = True
+
+
+ # Publishers
+ self.xavier_battery_level_pub = rospy.Publisher(
+ "/auv/battery_level/xavier", Float32, queue_size=1
+ )
+ if not self.is_PSM_fuckd_voltage:
+ self.system_battery_level_pub = rospy.Publisher(
+ "/auv/battery_level/system", Float32, queue_size=1
+ )
+ if not self.is_PSM_fuckd_current:
+ self.system_battery_current_draw_pub = rospy.Publisher(
+ "/auv/battery_level/system_current_draw", Float32, queue_size=1
+ )
+
+ # set up callbacks
+
+ self.xavier_timer = rospy.Timer(rospy.Duration(secs=xavier_interval), self.xavier_cb)
+ self.log_timer = rospy.Timer(rospy.Duration(secs=logging_interval), self.log_cb)
+
+ if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current:
+ self.system_timer = rospy.Timer(rospy.Duration(secs=system_interval), self.system_cb)
+ rospy.loginfo("BatteryMonitor initialized")
+ else:
+ rospy.logwarn("System Battery Monitoring from PSM is not available.")
+
+ def xavier_cb(self, event):
+ """Record output from voltage meter command, decode from bytes object to string, convert from string to integer"""
+ xavier_mV = int(
+ subprocess.check_output(["cat", self.path_to_xavier_measurement]).decode("utf-8")
+ )
+ self.xavier_voltage = xavier_mV / 1000.0
+
+ self.xavier_battery_level_pub.publish(self.xavier_voltage)
+ if not self.xavier_recieved:
+ self.xavier_recieved = True
+
+ def system_cb(self, event):
+ """Read voltage of system from powersense device."""
+
+ # MCP3425 address, 0x68(104)
+ # Read data back from 0x00(00), 2 bytes, MSB first
+ # raw_adc MSB, raw_adc LSB
+ if not self.is_PSM_fuckd_voltage:
+ voltage_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_voltage, 0x00, 2)
+
+ # Convert the data to 12-bits
+ raw_adc_voltage = (voltage_msg[0] & 0x0F) * 256 + voltage_msg[1]
+ if raw_adc_voltage > 2047:
+ raw_adc_voltage -= 4095
+
+ # MCP3425 address, 0x68(104)
+ # Read data back from 0x00(00), 2 bytes, MSB first
+ # raw_adc MSB, raw_adc LSB
+ if not self.is_PSM_fuckd_current:
+ current_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_current, 0x00, 2)
+ # Convert the data to 12-bits
+ raw_adc_current = (current_msg[0] & 0x0F) * 256 + current_msg[1]
+ if raw_adc_current > 2047:
+ raw_adc_current -= 4095
+
+ # PSM specific conversion ratio
+ if not self.is_PSM_fuckd_voltage:
+ self.system_voltage = raw_adc_voltage * 0.011
+ if not self.is_PSM_fuckd_current:
+ self.system_current = raw_adc_current * 0.0504
+
+ # publish
+ if not self.is_PSM_fuckd_voltage:
+ self.system_battery_level_pub.publish(self.system_voltage)
+ if not self.is_PSM_fuckd_current:
+ self.system_battery_current_draw_pub.publish(self.system_current)
+ if not self.system_recieved:
+ self.system_recieved = True
+
+ def log_cb(self, event):
+
+ if self.xavier_recieved:
+ self.log_voltage(self.xavier_voltage, "xavier")
+ else:
+ rospy.loginfo("No voltage recieved from Xavier yet.")
+
+ if self.system_recieved:
+ self.log_voltage(self.system_voltage, "system")
+ else:
+ rospy.loginfo("No voltage recieved from system yet.")
+
+
+ def log_voltage(self, voltage, title):
+
+ if voltage == 0:
+ rospy.loginfo("Voltage is zero. Killswitch is probably off.")
+
+ # Critical voltage level
+ elif voltage <= self.critical_level:
+ rospy.logerr("Critical %s voltage: %.3fV" % (title, voltage))
+
+ # Warning voltage level
+ elif voltage <= self.warning_level:
+ rospy.logwarn("%s voltage: %.3fV" % (title, voltage))
+
+ else:
+ rospy.loginfo("%s voltage: %.3fV" % (title, voltage))
+
+ def shutdown(self):
+ if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current:
+ self.system_timer.shutdown()
+ self.xavier_timer.shutdown()
+ self.log_timer.shutdown()
+ self.bus.close()
if __name__ == "__main__":
- bm = BatteryMonitor()
- try:
- rospy.spin()
- finally:
- bm.shutdown()
+ bm = BatteryMonitor()
+ try:
+ rospy.spin()
+ finally:
+ bm.shutdown()
diff --git a/mission/internal_status/src/battery_monitor.pye b/mission/internal_status/src/battery_monitor.pye
new file mode 100755
index 000000000..7fbcf9c93
--- /dev/null
+++ b/mission/internal_status/src/battery_monitor.pye
@@ -0,0 +1,182 @@
+#!/usr/bin/python3
+
+import subprocess
+import time
+import traceback
+
+import rospy
+import smbus
+from std_msgs.msg import Float32
+
+
+class BatteryMonitor:
+ def __init__(self):
+
+ rospy.init_node("battery_monitor")
+
+ # Parameters
+ self.path_to_xavier_measurement = rospy.get_param("/battery/xavier/path", default="/sys/bus/i2c/drivers/ina3221x/1-0040/iio:device0/in_voltage0_input")
+ self.path_to_powersense = rospy.get_param("/battery/system/path", default="/dev/i2c-8")
+
+ self.i2c_address_powersense_voltage = rospy.get_param("/i2c/psm/address_voltage", default=0x6a)
+ self.i2c_address_powersense_current = rospy.get_param("/i2c/psm/address_current", default=0x69)
+ self.i2c_bus_number = rospy.get_param("/i2c/psm/bus_number", default=8)
+ rospy.loginfo("PSM Voltage I2C address: '0x{:02x}'".format(self.i2c_address_powersense_voltage))
+ rospy.loginfo("PSM Current I2C address: '0x{:02x}'".format(self.i2c_address_powersense_current))
+ rospy.loginfo("PSM I2C bus number: '%d'", self.i2c_bus_number)
+
+ self.critical_level = rospy.get_param("/battery/thresholds/critical", default=13.5)
+ self.warning_level = rospy.get_param("/battery/thresholds/warning", default=14.5)
+
+ system_interval = rospy.get_param("/battery/system/interval", 0.05)
+ xavier_interval = rospy.get_param("/battery/xavier/interval", 10)
+ logging_interval = rospy.get_param("/battery/logging/interval", 10)
+
+ # Local variables
+ self.xavier_voltage = 0.0
+ self.system_voltage = 0.0
+ self.system_current = 0.0
+
+ self.xavier_recieved = False
+ self.system_recieved = False
+
+ self.is_PSM_fuckd_voltage = False
+ self.is_PSM_fuckd_current = False
+
+ # Get I2C bus for power sense module
+ self.bus = smbus.SMBus(self.i2c_bus_number)
+ time.sleep(1)
+
+ # Send configure command to the module to enable continuous conversion in 12-bit mode
+ try:
+ self.bus.write_byte(self.i2c_address_powersense_voltage, 0x10)
+ time.sleep(0.5)
+ except IOError:
+ print(traceback.format_exc())
+ self.is_PSM_fuckd_voltage = True
+
+ try:
+ self.bus.write_byte(self.i2c_address_powersense_current, 0x10)
+ time.sleep(0.5)
+ except IOError:
+ print(traceback.format_exc())
+ self.is_PSM_fuckd_current = True
+
+
+ # Publishers
+ self.xavier_battery_level_pub = rospy.Publisher(
+ "/auv/battery_level/xavier", Float32, queue_size=1
+ )
+ if not self.is_PSM_fuckd_voltage:
+ self.system_battery_level_pub = rospy.Publisher(
+ "/auv/battery_level/system", Float32, queue_size=1
+ )
+ if not self.is_PSM_fuckd_current:
+ self.system_battery_current_draw_pub = rospy.Publisher(
+ "/auv/battery_level/system_current_draw", Float32, queue_size=1
+ )
+
+ # set up callbacks
+
+ self.xavier_timer = rospy.Timer(rospy.Duration(secs=xavier_interval), self.xavier_cb)
+ self.log_timer = rospy.Timer(rospy.Duration(secs=logging_interval), self.log_cb)
+
+ if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current:
+ self.system_timer = rospy.Timer(rospy.Duration(secs=system_interval), self.system_cb)
+ rospy.loginfo("BatteryMonitor initialized")
+ else:
+ rospy.logwarn("System Battery Monitoring from PSM is not available.")
+
+ def xavier_cb(self, event):
+ """Record output from voltage meter command, decode from bytes object to string, convert from string to integer"""
+ xavier_mV = int(
+ subprocess.check_output(["cat", self.path_to_xavier_measurement]).decode("utf-8")
+ )
+ self.xavier_voltage = xavier_mV / 1000.0
+
+ self.xavier_battery_level_pub.publish(self.xavier_voltage)
+ if not self.xavier_recieved:
+ self.xavier_recieved = True
+
+ def system_cb(self, event):
+ """Read voltage of system from powersense device."""
+
+ # MCP3425 address, 0x68(104)
+ # Read data back from 0x00(00), 2 bytes, MSB first
+ # raw_adc MSB, raw_adc LSB
+ if not self.is_PSM_fuckd_voltage:
+ voltage_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_voltage, 0x00, 2)
+
+ # Convert the data to 12-bits
+ raw_adc_voltage = (voltage_msg[0] & 0x0F) * 256 + voltage_msg[1]
+ if raw_adc_voltage > 2047:
+ raw_adc_voltage -= 4095
+
+ # MCP3425 address, 0x68(104)
+ # Read data back from 0x00(00), 2 bytes, MSB first
+ # raw_adc MSB, raw_adc LSB
+ if not self.is_PSM_fuckd_current:
+ current_msg = self.bus.read_i2c_block_data(self.i2c_address_powersense_current, 0x00, 2)
+
+ # Convert the data to 12-bits
+ raw_adc_current = (current_msg[0] & 0x0F) * 256 + current_msg[1]
+ if raw_adc_current > 2047:
+ raw_adc_current -= 4095
+
+ # PSM specific conversion ratio
+ if not self.is_PSM_fuckd_voltage:
+ self.system_voltage = raw_adc_voltage * 0.011
+ if not self.is_PSM_fuckd_current:
+ self.system_current = raw_adc_current * 0.0504
+
+ # publish
+ if not self.is_PSM_fuckd_voltage:
+ self.system_battery_level_pub.publish(self.system_voltage)
+ if not self.is_PSM_fuckd_current:
+ self.system_battery_current_draw_pub.publish(self.system_current)
+ if not self.system_recieved:
+ self.system_recieved = True
+
+ def log_cb(self, event):
+
+ if self.xavier_recieved:
+ self.log_voltage(self.xavier_voltage, "xavier")
+ else:
+ rospy.loginfo("No voltage recieved from Xavier yet.")
+
+ if self.system_recieved:
+ self.log_voltage(self.system_voltage, "system")
+ else:
+ rospy.loginfo("No voltage recieved from system yet.")
+
+
+ def log_voltage(self, voltage, title):
+
+ if voltage == 0:
+ rospy.loginfo("Voltage is zero. Killswitch is probably off.")
+
+ # Critical voltage level
+ elif voltage <= self.critical_level:
+ rospy.logerr("Critical %s voltage: %.3fV" % (title, voltage))
+
+ # Warning voltage level
+ elif voltage <= self.warning_level:
+ rospy.logwarn("%s voltage: %.3fV" % (title, voltage))
+
+ else:
+ rospy.loginfo("%s voltage: %.3fV" % (title, voltage))
+
+ def shutdown(self):
+ if not self.is_PSM_fuckd_voltage or not self.is_PSM_fuckd_current:
+ self.system_timer.shutdown()
+ self.xavier_timer.shutdown()
+ self.log_timer.shutdown()
+ self.bus.close()
+
+
+if __name__ == "__main__":
+ bm = BatteryMonitor()
+ try:
+ rospy.spin()
+ finally:
+ bm.shutdown()
diff --git a/mission/internal_status/src/temperature_monitor.py b/mission/internal_status/src/temperature_monitor.py
index e95a6469b..26d3b7e03 100755
--- a/mission/internal_status/src/temperature_monitor.py
+++ b/mission/internal_status/src/temperature_monitor.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
#python imports
import subprocess
@@ -9,47 +9,47 @@
from std_msgs.msg import Int32
class TemperatureMonitor():
- def __init__(self):
-
- rospy.init_node("temperature_monitor")
+ def __init__(self):
+
+ rospy.init_node("temperature_monitor")
- # Settings
- self.interval = rospy.get_param("/temperature/logging/interval") # How often the battery level is checked and published
- self.temperature_template = r"\d*\.{0,1}\d*" # In python file because yaml doesn't like escape characters
-
- # Publisher
- self.cpu_temperature_pub = rospy.Publisher("/auv/temperature/cpu", Int32, queue_size=1)
- self.gpu_temperature_pub = rospy.Publisher("/auv/temperature/gpu", Int32, queue_size=1)
-
- # Start the tegrastats subprocess
- self.process = subprocess.Popen("tegrastats", shell=False, stdout=subprocess.PIPE)
+ # Settings
+ self.interval = rospy.get_param("/temperature/logging/interval") # How often the battery level is checked and published
+ self.temperature_template = r"\d*\.{0,1}\d*" # In python file because yaml doesn't like escape characters
+
+ # Publisher
+ self.cpu_temperature_pub = rospy.Publisher("/auv/temperature/cpu", Int32, queue_size=1)
+ self.gpu_temperature_pub = rospy.Publisher("/auv/temperature/gpu", Int32, queue_size=1)
+
+ # Start the tegrastats subprocess
+ self.process = subprocess.Popen("tegrastats", shell=False, stdout=subprocess.PIPE)
- def measure_temp(self):
+ def measure_temp(self):
- # Record output from temperature meter command, decode from bytes object to string, convert from string to integer
+ # Record output from temperature meter command, decode from bytes object to string, convert from string to integer
- stats = self.process.stdout.readlines()[-1].decode("utf-8")
+ stats = self.process.stdout.readlines()[-1].decode("utf-8")
- if stats != "":
+ if stats != "":
- cpu_search = re.search("CPU@("+self.temperature_template+")C", stats)
- gpu_search = re.search("GPU@("+self.temperature_template+")C", stats)
+ cpu_search = re.search("CPU@("+self.temperature_template+")C", stats)
+ gpu_search = re.search("GPU@("+self.temperature_template+")C", stats)
- self.cpu_temperature = int(float(cpu_search.group(1)))
- self.gpu_temperature = int(float(gpu_search.group(1)))
+ self.cpu_temperature = int(float(cpu_search.group(1)))
+ self.gpu_temperature = int(float(gpu_search.group(1)))
- def spin(self):
- # Main loop
- while not rospy.is_shutdown():
+ def spin(self):
+ # Main loop
+ while not rospy.is_shutdown():
- self.measure_temp()
- self.cpu_temperature_pub.publish(self.cpu_temperature)
- self.gpu_temperature_pub.publish(self.gpu_temperature)
+ self.measure_temp()
+ self.cpu_temperature_pub.publish(self.cpu_temperature)
+ self.gpu_temperature_pub.publish(self.gpu_temperature)
- rospy.sleep(self.interval)
+ rospy.sleep(self.interval)
if __name__ == "__main__":
- tm = TemperatureMonitor()
- tm.spin()
+ tm = TemperatureMonitor()
+ tm.spin()
diff --git a/mission/internal_status/src/temperature_monitor.pye b/mission/internal_status/src/temperature_monitor.pye
new file mode 100755
index 000000000..e2cca634f
--- /dev/null
+++ b/mission/internal_status/src/temperature_monitor.pye
@@ -0,0 +1,55 @@
+#!/usr/bin/python3
+
+#python imports
+import subprocess
+import re
+
+#ros imports
+import rospy
+from std_msgs.msg import Int32
+
+class TemperatureMonitor():
+ def __init__(self):
+
+ rospy.init_node("temperature_monitor")
+
+ # Settings
+ self.interval = rospy.get_param("/temperature/logging/interval") # How often the battery level is checked and published
+ self.temperature_template = r"\d*\.{0,1}\d*" # In python file because yaml doesn't like escape characters
+
+ # Publisher
+ self.cpu_temperature_pub = rospy.Publisher("/auv/temperature/cpu", Int32, queue_size=1)
+ self.gpu_temperature_pub = rospy.Publisher("/auv/temperature/gpu", Int32, queue_size=1)
+
+ # Start the tegrastats subprocess
+ self.process = subprocess.Popen("tegrastats", shell=False, stdout=subprocess.PIPE)
+
+ def measure_temp(self):
+
+ # Record output from temperature meter command, decode from bytes object to string, convert from string to integer
+
+ stats = self.process.stdout.readlines()[-1].decode("utf-8")
+
+ if stats != "":
+
+ cpu_search = re.search("CPU@("+self.temperature_template+")C", stats)
+ gpu_search = re.search("GPU@("+self.temperature_template+")C", stats)
+
+ self.cpu_temperature = int(float(cpu_search.group(1)))
+ self.gpu_temperature = int(float(gpu_search.group(1)))
+
+
+ def spin(self):
+ # Main loop
+ while not rospy.is_shutdown():
+
+ self.measure_temp()
+ self.cpu_temperature_pub.publish(self.cpu_temperature)
+ self.gpu_temperature_pub.publish(self.gpu_temperature)
+
+ rospy.sleep(self.interval)
+
+if __name__ == "__main__":
+
+ tm = TemperatureMonitor()
+ tm.spin()
diff --git a/mission/joystick_interface/scripts/joystick_interface.py b/mission/joystick_interface/scripts/joystick_interface.py
index 62f2b500e..8e127ea34 100755
--- a/mission/joystick_interface/scripts/joystick_interface.py
+++ b/mission/joystick_interface/scripts/joystick_interface.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
from enum import IntEnum
diff --git a/motion/dp_controller/cfg/Controller.cfg b/motion/dp_controller/cfg/Controller.cfg
index 84605befd..a713616ce 100755
--- a/motion/dp_controller/cfg/Controller.cfg
+++ b/motion/dp_controller/cfg/Controller.cfg
@@ -1,4 +1,4 @@
-#! /usr/bin/env python
+#! /usr/bin/python3
PACKAGE='dp_controller'
import roslib
diff --git a/motion/guidance_interface/src/guidance_interface.py b/motion/guidance_interface/src/guidance_interface.py
index b7cc36351..8f2704290 100755
--- a/motion/guidance_interface/src/guidance_interface.py
+++ b/motion/guidance_interface/src/guidance_interface.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
from enum import IntEnum, Enum
diff --git a/motion/joystick_guidance/scripts/joystick_guidance.py b/motion/joystick_guidance/scripts/joystick_guidance.py
index 92bd404f7..e14df4ba0 100755
--- a/motion/joystick_guidance/scripts/joystick_guidance.py
+++ b/motion/joystick_guidance/scripts/joystick_guidance.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Jae Hyeong Hwang and O. Solbo
# Copyright (c) 2021, Vortex NTNU.
diff --git a/motion/joystick_topside/scripts/joystick_topside.py b/motion/joystick_topside/scripts/joystick_topside.py
index 048abfd28..21946e19b 100755
--- a/motion/joystick_topside/scripts/joystick_topside.py
+++ b/motion/joystick_topside/scripts/joystick_topside.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# -*- coding: utf-8 -*-
# Written by Jae Hyeong Hwang and Christopher Strøm
diff --git a/motion/los_controller/cfg/LOSController.cfg b/motion/los_controller/cfg/LOSController.cfg
index 6ce78219a..c2906308d 100644
--- a/motion/los_controller/cfg/LOSController.cfg
+++ b/motion/los_controller/cfg/LOSController.cfg
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
PACKAGE = "los_controller"
from dynamic_reconfigure.parameter_generator_catkin import *
diff --git a/motion/los_controller/src/backstepping/backstepping_controller.py b/motion/los_controller/src/backstepping/backstepping_controller.py
index 7585119bd..d4199ddd3 100755
--- a/motion/los_controller/src/backstepping/backstepping_controller.py
+++ b/motion/los_controller/src/backstepping/backstepping_controller.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
# All rights reserved.
diff --git a/motion/los_controller/src/los_controller_node.py b/motion/los_controller/src/los_controller_node.py
index 6caa8d526..30e68cfb8 100755
--- a/motion/los_controller/src/los_controller_node.py
+++ b/motion/los_controller/src/los_controller_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Christopher Strom
# Copyright (c) 2020, Vortex NTNU.
# All rights reserved.
diff --git a/motion/los_controller/src/pid/pid_controller.py b/motion/los_controller/src/pid/pid_controller.py
index a6844dc40..f32ea5070 100755
--- a/motion/los_controller/src/pid/pid_controller.py
+++ b/motion/los_controller/src/pid/pid_controller.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
# All rights reserved.
diff --git a/motion/los_guidance/cfg/LOS.cfg b/motion/los_guidance/cfg/LOS.cfg
index b89c1e6ed..8f89d2d7c 100755
--- a/motion/los_guidance/cfg/LOS.cfg
+++ b/motion/los_guidance/cfg/LOS.cfg
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
PACKAGE = "los_guidance"
from dynamic_reconfigure.parameter_generator_catkin import *
diff --git a/motion/los_guidance/scripts/los_guidance_node.py b/motion/los_guidance/scripts/los_guidance_node.py
index 9f55e404b..2ba966236 100755
--- a/motion/los_guidance/scripts/los_guidance_node.py
+++ b/motion/los_guidance/scripts/los_guidance_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Documented by Christopher Strom and Jae Hyeong Hwang
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
diff --git a/motion/motion_launch/launch/motion.launch b/motion/motion_launch/launch/motion.launch
index 36986778c..51e987931 100644
--- a/motion/motion_launch/launch/motion.launch
+++ b/motion/motion_launch/launch/motion.launch
@@ -26,8 +26,8 @@
-
-
+
@@ -44,10 +44,10 @@
-
+
diff --git a/motion/motion_launch/launch/motion.launch.save b/motion/motion_launch/launch/motion.launch.save
new file mode 100644
index 000000000..7654592da
--- /dev/null
+++ b/motion/motion_launch/launch/motion.launch.save
@@ -0,0 +1,60 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/motion/reference_model/los_reference_model/scripts/discrete_tustin.py b/motion/reference_model/los_reference_model/scripts/discrete_tustin.py
index ce03cbf84..83dd71634 100755
--- a/motion/reference_model/los_reference_model/scripts/discrete_tustin.py
+++ b/motion/reference_model/los_reference_model/scripts/discrete_tustin.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
# All rights reserved.
diff --git a/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py b/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py
index 2c1009ce3..a16d4b0ac 100755
--- a/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py
+++ b/motion/reference_model/los_reference_model/scripts/los_reference_model_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
import numpy as np
diff --git a/motion/velocity_controller/cfg/velocity_controller.cfg b/motion/velocity_controller/cfg/velocity_controller.cfg
index c9412b0f1..de7c3b150 100755
--- a/motion/velocity_controller/cfg/velocity_controller.cfg
+++ b/motion/velocity_controller/cfg/velocity_controller.cfg
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
PACKAGE = "velocity_controller"
from dynamic_reconfigure.parameter_generator_catkin import *
diff --git a/motion/velocity_controller/scripts/velocity_config_server.py b/motion/velocity_controller/scripts/velocity_config_server.py
index 7a52c0ada..e01673579 100755
--- a/motion/velocity_controller/scripts/velocity_config_server.py
+++ b/motion/velocity_controller/scripts/velocity_config_server.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from dynamic_reconfigure.server import Server
diff --git a/motion/vtf_guidance_and_control/cfg/vtf_controller.cfg b/motion/vtf_guidance_and_control/cfg/vtf_controller.cfg
index 322bc6a79..325a1c2fd 100644
--- a/motion/vtf_guidance_and_control/cfg/vtf_controller.cfg
+++ b/motion/vtf_guidance_and_control/cfg/vtf_controller.cfg
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
PACKAGE = "vtf_guidance_and_control"
from dynamic_reconfigure.parameter_generator_catkin import *
diff --git a/motion/vtf_guidance_and_control/scripts/auv_model.py b/motion/vtf_guidance_and_control/scripts/auv_model.py
index d8e78e174..b72d33208 100755
--- a/motion/vtf_guidance_and_control/scripts/auv_model.py
+++ b/motion/vtf_guidance_and_control/scripts/auv_model.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
# Documentation can be found in my master's thesis: Guidance and Control System for Dynamic Positioning and PathFollowing of an AUV exposed to Ocean Currents
diff --git a/motion/vtf_guidance_and_control/scripts/auv_simulator.py b/motion/vtf_guidance_and_control/scripts/auv_simulator.py
index 14a4beedb..48c199836 100755
--- a/motion/vtf_guidance_and_control/scripts/auv_simulator.py
+++ b/motion/vtf_guidance_and_control/scripts/auv_simulator.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
import numpy as np
diff --git a/motion/vtf_guidance_and_control/scripts/control_allocation.py b/motion/vtf_guidance_and_control/scripts/control_allocation.py
index 43b5c3db3..b844a12b9 100755
--- a/motion/vtf_guidance_and_control/scripts/control_allocation.py
+++ b/motion/vtf_guidance_and_control/scripts/control_allocation.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
# Documentation can be found in my master's thesis: Guidance and Control System for Dynamic Positioning and PathFollowing of an AUV exposed to Ocean Currents
diff --git a/motion/vtf_guidance_and_control/scripts/control_system.py b/motion/vtf_guidance_and_control/scripts/control_system.py
index 671840632..d6d44afba 100755
--- a/motion/vtf_guidance_and_control/scripts/control_system.py
+++ b/motion/vtf_guidance_and_control/scripts/control_system.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
import numpy as np
diff --git a/motion/vtf_guidance_and_control/scripts/current_estimator.py b/motion/vtf_guidance_and_control/scripts/current_estimator.py
index 1301b21e9..52df9ba58 100755
--- a/motion/vtf_guidance_and_control/scripts/current_estimator.py
+++ b/motion/vtf_guidance_and_control/scripts/current_estimator.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
# Documentation can be found in my master's thesis:Guidance and Control System for Dynamic Positioning and PathFollowing of an AUV exposed to Ocean Currents
diff --git a/motion/vtf_guidance_and_control/scripts/functions.py b/motion/vtf_guidance_and_control/scripts/functions.py
index 4f4387e50..86bdda303 100755
--- a/motion/vtf_guidance_and_control/scripts/functions.py
+++ b/motion/vtf_guidance_and_control/scripts/functions.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
import numpy as np
diff --git a/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py b/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py
index c27dc4f37..4d383128c 100755
--- a/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py
+++ b/motion/vtf_guidance_and_control/scripts/guidance_and_control_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
import rospy
diff --git a/motion/vtf_guidance_and_control/scripts/path.py b/motion/vtf_guidance_and_control/scripts/path.py
index fd974676d..94325f005 100755
--- a/motion/vtf_guidance_and_control/scripts/path.py
+++ b/motion/vtf_guidance_and_control/scripts/path.py
@@ -1,5 +1,5 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
# Documentation can be found in my master's thesis: Guidance and Control System for Dynamic Positioning and PathFollowing of an AUV exposed to Ocean Currents
diff --git a/motion/vtf_guidance_and_control/scripts/virtual_target.py b/motion/vtf_guidance_and_control/scripts/virtual_target.py
index 5da92f4d3..f4a23ea37 100755
--- a/motion/vtf_guidance_and_control/scripts/virtual_target.py
+++ b/motion/vtf_guidance_and_control/scripts/virtual_target.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Aksel Kristoffersen
# Documentation can be found in my master's thesis, chapter __ and __
diff --git a/motion/vtf_guidance_and_control/scripts/vtf_server.py b/motion/vtf_guidance_and_control/scripts/vtf_server.py
index 4230152aa..9536a61ef 100755
--- a/motion/vtf_guidance_and_control/scripts/vtf_server.py
+++ b/motion/vtf_guidance_and_control/scripts/vtf_server.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
diff --git a/navigation/compass/src/compass.py b/navigation/compass/src/compass.py
index 64d445d5f..2f8cad9a0 100755
--- a/navigation/compass/src/compass.py
+++ b/navigation/compass/src/compass.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
from math import atan2, degrees
diff --git a/navigation/fake_odometry_filtered/scripts/odom.py b/navigation/fake_odometry_filtered/scripts/odom.py
index 27f5b3b5c..9f17aed72 100755
--- a/navigation/fake_odometry_filtered/scripts/odom.py
+++ b/navigation/fake_odometry_filtered/scripts/odom.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
from nav_msgs.msg import Odometry
diff --git a/navigation/inspect_point/src/inspect_point.py b/navigation/inspect_point/src/inspect_point.py
index c52b701d3..039cbe8ee 100755
--- a/navigation/inspect_point/src/inspect_point.py
+++ b/navigation/inspect_point/src/inspect_point.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
#python imports
import rospy
diff --git a/navigation/sensor_interface/scripts/bno055_interface.py b/navigation/sensor_interface/scripts/bno055_interface.py
index 91a30e17f..7745c0206 100755
--- a/navigation/sensor_interface/scripts/bno055_interface.py
+++ b/navigation/sensor_interface/scripts/bno055_interface.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import csv
import rospkg
diff --git a/navigation/sensor_interface/scripts/ms5837.py b/navigation/sensor_interface/scripts/ms5837.py
index 4544923bf..0d621953b 100644
--- a/navigation/sensor_interface/scripts/ms5837.py
+++ b/navigation/sensor_interface/scripts/ms5837.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
try:
import smbus
diff --git a/navigation/sensor_interface/scripts/ms5837_interface.py b/navigation/sensor_interface/scripts/ms5837_interface.py
index aefc96877..271fb8c09 100755
--- a/navigation/sensor_interface/scripts/ms5837_interface.py
+++ b/navigation/sensor_interface/scripts/ms5837_interface.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import rospy
import ms5837
diff --git a/navigation/waypoint_action_client/scripts/.load_waypoints.py.swo b/navigation/waypoint_action_client/scripts/.load_waypoints.py.swo
index 633bf5df0..f12c6587c 100644
Binary files a/navigation/waypoint_action_client/scripts/.load_waypoints.py.swo and b/navigation/waypoint_action_client/scripts/.load_waypoints.py.swo differ
diff --git a/navigation/waypoint_action_client/scripts/load_waypoints.py b/navigation/waypoint_action_client/scripts/load_waypoints.py
index 64cee0482..e70607ddb 100755
--- a/navigation/waypoint_action_client/scripts/load_waypoints.py
+++ b/navigation/waypoint_action_client/scripts/load_waypoints.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
# All rights reserved.
diff --git a/navigation/waypoint_action_client/scripts/old/move_base_square.py b/navigation/waypoint_action_client/scripts/old/move_base_square.py
index 7f65e8128..1b7ff6fd4 100755
--- a/navigation/waypoint_action_client/scripts/old/move_base_square.py
+++ b/navigation/waypoint_action_client/scripts/old/move_base_square.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
import roslib; roslib.load_manifest('waypoint_action_client')
import rospy
diff --git a/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py b/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py
index a31cd12d2..4ffb1a36c 100755
--- a/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py
+++ b/navigation/waypoint_action_client/scripts/old/static_waypoint_client.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2019 Manta AUV, Vortex NTNU.
# All rights reserved.
diff --git a/navigation/waypoint_action_client/scripts/waypoint_tracking_client.py b/navigation/waypoint_action_client/scripts/waypoint_tracking_client.py
index 895f03c65..d78f3c837 100755
--- a/navigation/waypoint_action_client/scripts/waypoint_tracking_client.py
+++ b/navigation/waypoint_action_client/scripts/waypoint_tracking_client.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/python3
# Written by Kristoffer Rakstad Solberg, Student
# Copyright (c) 2020 Manta AUV, Vortex NTNU.
# All rights reserved.