Im trying to implement multi threading (parallel processing) with python and using mutex threading. I have first process that check the Pressure Value and the modem update(in the code implemented with odom_callback and callback_modem functions), and second process that calls ROS SERVICES ( in the code implemented with ros_serice_server server and imu_client client functions). Here is the implementation code in python
#!/usr/bin/env python3
from __future__ import print_function
import rospy
import numpy as np
from os import system
import time
import threading
import Microcontroller_Manager_Serial as Serial
import IMU_Functions as IMU
import Motors_Functions as Motor
import Pressure_Functions as Pressure
from geometry_msgs.msg import Vector3
import Modem_Functions as Modem
import threading
import time
import serial
import serial.tools.list_ports
from time import sleep
from std_msgs.msg import Float32
from std_msgs.msg import String
from demo_teleop.srv import ImuValue,ImuValueResponse
P0 = 1.01325 #Default Pressure
mutex = threading.Lock()
Communication_Mode_ = 0
pub_pressure = rospy.Publisher('depth',Vector3,queue_size=1)
pub_modem = rospy.Publisher('modem_data',Float32,queue_size=1)
def handle_ros_services(req):
mutex.acquire(blocking=True)
print("Server Read Data:")
global T0
data_received = Pressure.Pressure_Get_Final_Values(1,1)
#print("Server Read Data:")
T0 = (np.int16((data_received[6]<<24) | (data_received[7]<<16) | (data_received[8]<<8) | (data_received[9])))/10000
T=T0
temperature = T
current_x_orientation_s = temperature
print("Returning Service Temperature Data", current_x_orientation_s)
return ImuValueResponse(current_x_orientation_s, True)
mutex.release()
def ros_serice_server():
s = rospy.Service('imu_value', ImuValue, handle_ros_services)
print("Ready to get_value")
def odom_callback():
# reentrang processing
mutex.acquire(blocking=True)
# work serial port here, e.g. send msg to serial port
global P0
data_received = Pressure.Pressure_Get_Final_Values(1,1)
#P1 = (np.int16((data_received_pressure[6]<<24) | (data_received_pressure[7]<<16) | (data_received_pressure[8]<<8) | (data_received_pressure[9])))/10000
P1 = (np.int16((data_received[6]<<24) | (data_received[7]<<16) | (data_received[8]<<8) | (data_received[9])))/10000
#P0 = (np.int16((data_received_pressure[6]<<24) | (data_received_pressure[7]<<16) | (data_received_pressure[8]<<8) | (data_received_pressure[9])))/10000
P0 = (np.int16((data_received[6]<<24) | (data_received[7]<<16) | (data_received[8]<<8) | (data_received[9])))/10000
P = P0 # Relative Measured Pressure
feedback =Vector3()
feedback.x = 0 #Angular Velocity
feedback.y = 0
feedback.z = P/9.81 #Depth
pressure = feedback.z
print("Pressure : ", pressure)
pub_pressure.publish(feedback)
# reentrant processing
mutex.release()
def callback_modem(event):
# reentrant processing
mutex.acquire(blocking=True)
# work serial port here, e.g. check for incoming data
event = Serial.Serial_Port_Receive_Data(20,0.2)
if (event == 1): # Received data from acoustic modem
modem_data= event
pub_modem.publish(modem_data)
print("received ")
else:
print("not received...... ")
mutex.release()
if __name__ == '__main__':
# initialize serial port here
Serial.Serial_Port_Standard()
rospy.init_node('imu_value')
ros_serice_server()
rospy.Timer(rospy.Duration(1), callback_modem)
while not rospy.is_shutdown():
try:
odom_callback()
except:
print('pass')
And the client node
#!/usr/bin/env python3
from __future__ import print_function
import rospy
import sys
import numpy as np
from os import system
import threading
import Microcontroller_Manager_Serial as Serial
import IMU_Functions as IMU
import Motors_Functions as Motor
import Pressure_Functions as Pressure
from geometry_msgs.msg import Vector3
import Modem_Functions as Modem
import time
import serial
import serial.tools.list_ports
from time import sleep
from std_msgs.msg import Float32
from std_msgs.msg import String
from demo_teleop.srv import *
mutex = threading.Lock()
Communication_Mode_ = 0
pub_modem = rospy.Publisher('modem_data',Float32,queue_size=1)
def imu_client():
mutex.acquire(blocking=True)
rospy.wait_for_service('imu_value')
imu_value = rospy.ServiceProxy('imu_value', ImuValue)
print("Request call send")
resp1 = imu_value(0.05)
return resp1.current_x_orientation_s
mutex.release()
if __name__ == "__main__":
rospy.init_node('client_node_f')
while not rospy.is_shutdown():
try:
print("entering client")
value = imu_client()
print(value)
time.sleep(1)
except:
print('pass')
So the output is following. The output of the first process with the ROS Services Server is
Pressure : 0.10602446483180428
Server Read Data:
Returning Service Temperature Data 1.0401
And then after calling the client I got
entering client
Request call send
1.0401
entering client
The problem is that after calling the ROS SERVICE client node the process stop so doesn't continue with the first process (Pressure value and modem update) . The ROS SERVICES process should be call only on demand and should HALT the first process (Pressure and modem) and then is should resume with the work. So, do I need to implement SEMAPHORES for the ROS SERVICES call ? If yes how it should be in the code. So I do need kind of synchronization , right?Please any help?
Your problem is:
def handle_ros_services(req):
mutex.acquire(blocking=True)
...
return ImuValueResponse(current_x_orientation_s, True)
mutex.release()
Because of the return statement, the release is never executed.
You need at the end:
value = ImValueResponse(...)
mutex.release()
return value
Even better would be to use your mutex as part of a with statement:
with mutex:
do anything you want, knowing that the lock will be released
at the end, even if you return or throw an exception.
Related
I'm making a project with a raspberry pi 4 and a NEO 6M gps module. I use pubnub to communicate from the raspberry pi to my self made website with a google map. The problem is that google maps crashes everytime I run the following script:
import RPi.GPIO as GPIO
import time
import board
import busio
import adafruit_gps
import pubnub
from pubnub.pnconfiguration import PNConfiguration
from pubnub.pubnub import PubNub
from pubnub.callbacks import SubscribeCallback
from pubnub.enums import PNOperationType, PNStatusCategory
def publish_callback(result, status):
pass
pnconfig = PNConfiguration()
pnconfig.subscribe_key = ""
pnconfig.publish_key = ""
pnconfig.ssl = False
pubnub = PubNub(pnconfig)
import serial
uart = serial.Serial("/dev/ttyAMA0", baudrate=9600, timeout=10)
gps = adafruit_gps.GPS(uart, debug=False)
gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
gps.send_command(b"PMTK220,1000")
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
last_print = time.monotonic()
while True:
gps.update()
current = time.monotonic()
if current - last_print >= 1.0:
last_print = current
if not gps.has_fix:
print("Waiting for fix...")
continue
print("=" * 40)
print(
"Fix timestamp: {}/{}/{} {:02}:{:02}:{:02}".format(
gps.timestamp_utc.tm_mon,
gps.timestamp_utc.tm_mday,
gps.timestamp_utc.tm_year,
gps.timestamp_utc.tm_hour,
gps.timestamp_utc.tm_min,
gps.timestamp_utc.tm_sec,
)
)
dictionary = {"latitude": gps.latitude, "longitude": gps.longitude}
pubnub.publish().channel("CHANNEL").message(dictionary).pn_async(publish_callback)
print("Latitude: {0:.6f} degrees".format(gps.latitude))
print("Longitude: {0:.6f} degrees".format(gps.longitude))
print("Fix quality: {}".format(gps.fix_quality))
if gps.satellites is not None:
print("# satellites: {}".format(gps.satellites))
if gps.altitude_m is not None:
print("Altitude: {} meters".format(gps.altitude_m))
if gps.speed_knots is not None:
print("Speed: {} knots".format(gps.speed_knots))
if gps.track_angle_deg is not None:
print("Track angle: {} degrees".format(gps.track_angle_deg))
if gps.horizontal_dilution is not None:
print("Horizontal dilution: {}".format(gps.horizontal_dilution))
if gps.height_geoid is not None:
print("Height geo ID: {} meters".format(gps.height_geoid))
What I want to do is set a delay on the lat & long that gets send to pubnub so it doesn't crash Google maps. Does anybody know how I should do that? Any help would be appreciated!
So I'm doing a project where I want my Raspberry Pi 4 to communicate through Modbus. I've bought a shield to enable RS485 communication from the Pi and I've been modifying the demo code (Software/Test Codes/MODBUS/rtumaster.py) and have been using a slave simulator on my computer to test.
I'm reaching out because I've gotten stuck on trying to read data from my slave. The command seems to go through okay (or the slave simulator doesn't complain is perhaps better to say), but I don't know how to get the data into my program. I've been trying something like this:
read = logger.info(master.execute(1, cst.READ_INPUT_REGISTERS, 5, 1))
And then trying to print that value to check, but it just returns 'None' instead of my value.
Any help is appreciated.
Current code:
## To install dependencies:
## sudo pip3 install modbus-tk
##################################################################################################
import serial
import fcntl
import os
import struct
import termios
import array
#import modbus lib
import modbus_tk
import modbus_tk.defines as cst
import modbus_tk.modbus as modbus
#import modbus_tk.modbus_rtu as modbus_rtu
from modbus_tk import modbus_rtu
# RS485 ioctls define
TIOCGRS485 = 0x542E
TIOCSRS485 = 0x542F
SER_RS485_ENABLED = 0b00000001
SER_RS485_RTS_ON_SEND = 0b00000010
SER_RS485_RTS_AFTER_SEND = 0b00000100
SER_RS485_RX_DURING_TX = 0b00010000
# rs 485 port
ser1 = serial.Serial("/dev/ttySC0",19200)
ser2 = serial.Serial("/dev/ttySC1",9600)
def rs485_enable():
buf = array.array('i', [0] * 8) # flags, delaytx, delayrx, padding
#enable 485 chanel 1
fcntl.ioctl(ser1, TIOCGRS485, buf)
buf[0] |= SER_RS485_ENABLED|SER_RS485_RTS_AFTER_SEND
buf[1] = 0
buf[2] = 0
fcntl.ioctl(ser1, TIOCSRS485, buf)
#enable 485 chanel 2
fcntl.ioctl(ser2, TIOCGRS485, buf)
buf[0] |= SER_RS485_ENABLED|SER_RS485_RTS_AFTER_SEND
buf[1] = 0
buf[2] = 0
fcntl.ioctl(ser2, TIOCSRS485, buf)
#end of rs485_enable():
if __name__ == '__main__':
logger = modbus_tk.utils.create_logger("console")
rs485_enable()
#set modbus master
master = modbus_rtu.RtuMaster(
serial.Serial(port= '/dev/ttySC0',
baudrate=9600,
bytesize=8,
parity='N',
stopbits=1,
xonxoff=0)
)
master.set_timeout(5.0)
master.set_verbose(True)
logger.info("connected")
read = logger.info(master.execute(1, cst.READ_INPUT_REGISTERS, 5, 1))
print("Value is: ", read)
This is the code which works well for all AT commands except "FN":
from digi.xbee.devices import XBeeDevice
#Initialise a serial port for the local xbee
local_xbee = XBeeDevice("/dev/tty.usbserial-AH02D9Q4", 9600).
#Opens serial port for sending commands
local_xbee.open()
#Sets new timeout for sync command operation
local.set_sync_ops_timeout(10).
#Send "FN" AT command to local xbee to receive neighbour list
neighbour_xbee_list = local.get_parameter("FN")
print(neighbour_xbee_list)
local_xbee.close()
Note:
The above code returns only one neighbour whereas I have more than one nodes in the network.
I believe the point of the question is not how to use the FN command, but how to discover neighbors.
For this, you can use the start_discovery_process function described here.
http://xbplib.readthedocs.io/en/latest/user_doc/discovering_the_xbee_network.html#discovernetwork
import serial
from digi.xbee.packets.common import ATCommPacket
from digi.xbee.devices import XBeeDevice
from digi.xbee.reader import PacketListener
from digi.xbee.serial import XBeeSerialPort
from digi.xbee.util import utils
import time
local_xbee = XBeeDevice("/dev/tty.usbserial-AH02D9Q4", 9600)
local_xbee.open()
print("This is : ", local_xbee.get_node_id())
print(local_xbee._packet_listener.is_running())
parameter = "FN"
frame_id = 33
my_packet = ATCommPacket(frame_id, parameter)
#print(my_packet)
#print(my_packet.frame_id)
#print(my_packet.command)
final_send = my_packet.output()
local_xbee._serial_port.write(final_send)
print("Finding Neighbours")
while True:
print(".")
Queue = local_xbee._packet_listener.get_queue()
received_packet = Queue.get_by_id(frame_id)
if received_packet != None:
#if received_packet.status == ATCommandStatus.OK:
final = received_packet._get_api_packet_spec_data().__str__()
print(final)
time.sleep(0.5)
local_xbee.close()
I am using a USB microwave source, which communicates via a virtual COM port.
Communication is done via python. Everything works fine, as long as I am executing the code blockingly.
However, as soon as any communication is done in a thread
I get a SerialException: Attempting to use a port that is not open. Is there an obvious reason, why this is happening? Nothing else, at least originating from my software, is trying to communicate with the port during that time.
The highest level script:
from serial import SerialException
import deer
import windfreak_usb
try:
deer.mw_targets = windfreak_usb.WindfreakSynthesizer()
except SerialException:
deer.mw_targets.port.close()
deer.mw_targets = windfreak_usb.WindfreakSynthesizer()
deer_mes = deer.DeerMeasurement(f_start=0.9e9,
f_end=1.2e9,
df=3e6,
f_nv=1.704e9,
seq=["[(['mw'],28),([],tau),(['mw', 'mwy'],56),([],tau),(['mw'],28),([],100)]",
"[(['mw'],28),([],tau),(['mw', 'mwy'],56),([],tau),(['mw'],84),([],100)]"],
power_nv=10,
power_targets=3,
tau=700
)
deer_mes.run(10e6) # <- this works perfectly, as it is the blocking version
# deer_mes.start(10e6) # <- raises the SerialException at the line indicated below
deer.mw_targets.port.close()
A reduced form of the microwave source (windfreak_usb.py):
import serial
import synthesizer
class WindfreakSynthesizer(synthesizer.Synthesizer):
def __init__(self):
synthesizer.Synthesizer.__init__(self)
self.port = serial.Serial(
port='COM14',
baudrate=9600,
timeout=10
)
self.off()
def __del__(self):
self.port.close()
def off(self):
self.port.write('o0')
def power(self, p):
p = int(p)
self.port.write('a{}'.format(p))
A reduced form of the measurement class (deer.py):
import threading
import time
import numpy
import os
from PyQt4 import QtCore
from PyQt4.QtCore import QObject
import matplotlib.pyplot as plt
import hardware
import matpickle
import pulsed
import pulser
import savepath
import synthesizer
if 'pg' not in globals():
pg = pulser.Pulser()
if 'mw_targets' not in globals():
mw_targets = synthesizer.Synthesizer()
timeout = 30
CurrentMeasurement = None # global variable pointing to the currently active measurement
class DeerMeasurement(QObject):
update = QtCore.pyqtSignal()
def __init__(self, f_start, f_end, df, f_nv, seq, power_nv, power_targets, tau, sweeps_per_iteration=50e3,
switching_time=300e-6):
super(QObject, self).__init__()
""" setting all parameters as self.parameter """
self.power_targets = power_targets
self.fs = numpy.arange(f_start, f_end + df, df)
self.abort = threading.Event()
self.save_deer()
def run(self, sweeps):
global CurrentMeasurement
if CurrentMeasurement is not None:
print('Deer Warning: cannot start measurement while another one is already running. Returning...')
return
CurrentMeasurement = self
# Start measurement
print('Deer measurement started.')
mw_targets.power(self.power_targets) # <- This causes the SerialException.
""" Here comes the actual measurement, that is never executed, as the line above kills the thread already with the SerialException. """
def start(self, sweeps, monitor=None):
"""Start Measurement in a thread."""
if monitor is not None:
monitor.register(self)
if not hasattr(self, 'mes_thread'):
# noinspection PyAttributeOutsideInit
self.mes_thread = threading.Thread(target=self.run, args=(sweeps,))
self.mes_thread.start()
else:
print('Already threading')
Any help is highly appreciated, as running the measurement outside a thread is not an option.
Best regards!
I would like to use char=getch.getch() in a loop (while(1)).
The problem when I use this like that it block my loop:
import getch
while(1):
char=getch.getch()
a=read_data()
if (char=='a'): c=....
if (char=='b'): c=....
If I don't put anything, my loop is blocked... How can I solve this for getting event from my keyboard?
EDIT : At the top is an example of what I want to do but if you are interested, my real script is here. I am actually working on an analyser spectrum and I want to scan by pressing some keys:
from pylab import *
from rtlsdr import *
from bluetooth import *
import sys
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import getch
sdr=RtlSdr()
#configure device
sdr.center_freq=double(sys.argv[1]) # centrale frequency
sdr.gain=double(sys.argv[2]) #gain
sdr.sample_rate=double(sys.argv[3]) # sample rate
#configure PSD
NFFT=int(sys.argv[4]) # nb points
# Bluetooth connexion
server_sock=BluetoothSocket( RFCOMM )
server_sock.bind(("",PORT_ANY))
server_sock.listen(1)
port = server_sock.getsockname()[1]
uuid="94f39d29-7d6d-437d-973b-fba39e49d4ee"
client_sock, client_info=server_sock.accept()
while (1):
samples=sdr.read_samples(256*1024)
result=psd(samples,NFFT,Fs=sdr.sample_rate/1e6,Fc=sdr.center_freq*1e6/1e6)
tab_freq=(result[1]/1e6)
value_freq=str(tab_freq)[1:-1]
value_list=[format(float(v), ".5f") for v in value_freq.split()]
value_freq2= "\n".join(value_list)
tab_pxx=result[0]
value_pxx=str(tab_pxx)[1:-1]
value_list2=[format(float(v), ".7f") for v in value_pxx.split()]
value_pxx2= "\n".join(value_list2)
client_sock.send(value_freq2+'\n'+'\n'.join(value_pxx2.split()))
char=getch.getch()
if (char=='a'):
sdr.center_freq=sdr.center_freq+0.1e6
print 'center_freq+'
if (char=='z'):
sdr.center_freq=sdr.center_freq-0.1e6
print 'center_freq-'
If you are on Windows, you can use msvcrt.kbhit() to see if there's a keypress waiting without blocking:
import msvcrt
import time
while True:
time.sleep(1)
if msvcrt.kbhit():
# Only if there's a keypress waiting do we get it with getch()
print "Key hit! ({})".format(msvcrt.getch())
else:
# Do something else here
print "Nothing..."
On Linux, it's more complicated because there is no kbhit() equivalent.