From: Federica Di Lauro <federicadilauro1998@gmail.com>
Date: Tue, 28 Jan 2020 09:20:19 +0000 (+0100)
Subject: fix python scripts, now looping till we can open serial port
X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=d89c52fccacfdc64d7339c434917aad47ad624f7;p=pioneer-stm32

fix python scripts, now looping till we can open serial port
---

diff --git a/utils/catkin_ws/joypad.launch b/utils/catkin_ws/joypad.launch
index 69f8ec6..e3765a8 100644
--- a/utils/catkin_ws/joypad.launch
+++ b/utils/catkin_ws/joypad.launch
@@ -1,5 +1,5 @@
 <launch>
   <node name="joy_node" pkg="joy" type="joy_node" />
   <node name="joy_to_cmd_vel" pkg="joypad_bridge" type="joy_to_cmd_vel.py"/>
-  <node name="cmd_vel_transmitter" pkg="joypad_bridge" type="cmd_vel_transmitter.py"/>
+  <node name="cmd_vel_transmitter" pkg="joypad_bridge" type="cmd_vel_transmitter.py" output="screen"/>
 </launch>
diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py b/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py
index ae2859c..381ae56 100755
--- a/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py
+++ b/utils/catkin_ws/src/joypad_bridge/scripts/cmd_vel_transmitter.py
@@ -2,36 +2,35 @@
 
 import rospy, serial, struct, time
 from geometry_msgs.msg import Twist
+from serial import SerialException
 
 ser = serial.Serial(
-	port='/dev/ttyUSB0',
         baudrate=115200,
         parity=serial.PARITY_NONE,
         stopbits=serial.STOPBITS_ONE,
         bytesize=serial.EIGHTBITS,
         rtscts=False)
 
-current_millis = 0 
-last_millis = 0
-
 def callback(data):
-    global current_millis
-    global last_millis
-    #da fixare
     linear = -data.linear.x
-    angular = -data.angular.z
-    last_millis = current_millis
-    current_millis = int(round(time.time() * 1000))
+    angular = -data.angular.z      #da fixare?
     rospy.loginfo('I heard %f %f', linear, angular)
-    if (1):
-        msg_output = struct.pack('<ff', linear, angular)
-        ser.write(msg_output)
-        ser.flush()
+    msg_output = struct.pack('<ff', linear, angular)
+    ser.write(msg_output)
+    ser.flush()
     
 
 
 def listener():
-
+    while(ser.is_open == False):
+        try:
+            ser.port = '/dev/ttyUSB0'
+            ser.open()
+        except SerialException:
+            print('couldnt open /dev/ttyUSB0')
+            time.sleep(2)
+    
+    print('ttyUSB0 opened')
     rospy.init_node('cmd_vel_transmitter', anonymous=True)
 
     rospy.Subscriber('/cmd_vel', Twist, callback)
diff --git a/utils/pid_tuning/pid_tuning.py b/utils/pid_tuning/pid_tuning.py
index 21af19a..eeaf5a7 100644
--- a/utils/pid_tuning/pid_tuning.py
+++ b/utils/pid_tuning/pid_tuning.py
@@ -1,11 +1,23 @@
 import serial, struct, time
+from serial import SerialException
+
 ser = serial.Serial(
-	port='/dev/ttyUSB0',
         baudrate=115200,
-        parity=serial.PARITY_ODD,
+        parity=serial.PARITY_NONE,
         stopbits=serial.STOPBITS_ONE,
         bytesize=serial.EIGHTBITS,
         rtscts=False)
+
+while (ser.is_open == False):
+	try:
+		ser.port = '/dev/ttyUSB0'
+		ser.open()
+	except SerialException:
+		print("couldn't open ttyUSB0, check the connection")
+		time.sleep(2)
+print("yay")
+
+
 pid_select = float(input("Enter 1 for left tuning, 2 for right tuning, 3 for cross tuning: "))
 if (pid_select == 3):
 	pid_lin_vel = float(input("Enter the linear velocity setpoint: "))
diff --git a/utils/py_serial_examples/receive.py b/utils/py_serial_examples/receive.py
index 182e3f4..94fc687 100644
--- a/utils/py_serial_examples/receive.py
+++ b/utils/py_serial_examples/receive.py
@@ -1,11 +1,20 @@
-import serial, struct, time
+import time, serial, struct
+from serial import SerialException
 ser = serial.Serial(
-	port='/dev/ttyUSB0',
         baudrate=115200,
-        parity=serial.PARITY_ODD,
+        parity=serial.PARITY_NONE,
         stopbits=serial.STOPBITS_ONE,
         bytesize=serial.EIGHTBITS,
         rtscts=False)
+while (ser.is_open == False):
+	try:
+		ser.port = '/dev/ttyUSB0'
+		ser.open()
+	except SerialException:
+		print("couldn't open ttyUSB0, check the connection")
+		time.sleep(2)
+
+print("port open")
 
 while 1:
 	ser.reset_input_buffer()
diff --git a/utils/py_serial_examples/transmit.py b/utils/py_serial_examples/transmit.py
index ce9df64..63f9bc4 100644
--- a/utils/py_serial_examples/transmit.py
+++ b/utils/py_serial_examples/transmit.py
@@ -1,11 +1,21 @@
-import serial, struct, time
+import time, serial, struct
+from serial import SerialException
+
 ser = serial.Serial(
-	port='/dev/ttyUSB0',
         baudrate=115200,
-        parity=serial.PARITY_ODD,
+        parity=serial.PARITY_NONE,
         stopbits=serial.STOPBITS_ONE,
         bytesize=serial.EIGHTBITS,
         rtscts=False)
+while (ser.is_open == False):
+	try:
+		ser.port = '/dev/ttyUSB0'
+		ser.open()
+	except SerialException:
+		print("couldn't open ttyUSB0, check the connection")
+		time.sleep(2)
+
+print("open port")
 ang_vel_cmd = 0
 lin_vel_cmd = 1
 while 1:
@@ -13,7 +23,5 @@ while 1:
 	ser.write(msg_output_buffer)
 	print(ang_vel_cmd)
 	print(lin_vel_cmd)
-#	ang_vel_cmd = ang_vel_cmd - 1
-#	lin_vel_cmd = lin_vel_cmd - 1
 	ser.reset_output_buffer()
 	time.sleep(3)
diff --git a/utils/ticks_calibration/ticks_logger.py b/utils/ticks_calibration/ticks_logger.py
index 341d872..4c375f3 100644
--- a/utils/ticks_calibration/ticks_logger.py
+++ b/utils/ticks_calibration/ticks_logger.py
@@ -1,14 +1,23 @@
 import serial, struct, time, signal, sys
 from datetime import datetime
+from serial import SerialException
 
 ser = serial.Serial(
-	port='/dev/ttyUSB0',
         baudrate=115200,
-        parity=serial.PARITY_ODD,
+        parity=serial.PARITY_NONE,
         stopbits=serial.STOPBITS_ONE,
         bytesize=serial.EIGHTBITS,
         rtscts=False)
 
+while (ser.is_open == False):
+	try:
+		ser.port = '/dev/ttyUSB0'
+		ser.open()
+	except SerialException:
+		print("couldn't open ttyUSB0, check the connection")
+		time.sleep(2)
+print("yay")
+
 def signal_handler(sig, frame):
 	print('Logging stopped')
 	print('Info logged on ' + file_name)