diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index c2815be629cbe704bc81afe0a7c810441fb93951..0343be7262ccf487703d207bbfbaeb2cdb691335 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index c64cd2666bf3ac1fc382e94118990a47ea8f2da4..f6731d07ba20184911cde95e4cb80c7a95727034 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -25,6 +25,7 @@ from sys import exc_info
from types import NoneType
from os import getpid
from functools import partial
+import pickle
"""
general notes
@@ -1016,23 +1017,33 @@ class ProcessManager:
self.data_queue = Queue()
self.side_process = Process(target=side_function,
args=(args, init_command, self.command_queue, self.data_queue,))
- # both ways, with shared memory for commands
+ # shared memory both ways
+ # one lock because i'm lazy
+ # but also, it's just copy-pasting
+ # we're in python, and NOW do we get picky with copy-pasting???????
if comm_direction == 3:
- self.data_queue = Queue()
# "sending" the command via shared memory
shm_name = "command"
- self.shm = shared_memory.SharedMemory(shm_name, create=True, size=init_command.nbytes)
- self.shared_command = np.ndarray(init_command.shape, dtype=init_command.dtype, buffer=self.shm.buf)
+ self.shm_cmd = shared_memory.SharedMemory(shm_name, create=True, size=init_command.nbytes)
+ self.shared_command = np.ndarray(init_command.shape, dtype=init_command.dtype, buffer=self.shm_cmd.buf)
self.shared_command[:] = init_command[:]
+ # same lock for both
self.shared_command_lock = Lock()
+ # getting data via different shared memory
+ shm_data_name = "data"
+ # size is chosen arbitrarily but 10k should be more than enough for anything really
+ self.shm_data = shared_memory.SharedMemory(shm_data_name, create=True, size=10000)
+ # initialize empty
+ p = pickle.dumps(None)
+ self.shm_data.buf[:len(p)] = p
# the process has to create its shared memory
self.side_process = Process(target=side_function,
- args=(args, init_command, shm_name, self.shared_command_lock, self.data_queue,))
+ args=(args, init_command, shm_name, self.shared_command_lock, self.shm_data,))
if type(side_function) == partial:
self.side_process.name = side_function.func.__name__
else:
self.side_process.name = side_function.__name__ + "_process"
- self.lastest_data = init_value
+ self.latest_data = init_value
self.side_process.start()
if self.args.debug_prints:
@@ -1090,7 +1101,7 @@ class ProcessManager:
#self.command_queue.get_nowait()
self.command_queue.put_nowait(command)
- # TODO: implement
+ # TODO merge with send command under if which depends on comm_direction
def sendCommandViaSharedMemory(self, command):
"""
sendCommandViaSharedMemory
@@ -1109,14 +1120,19 @@ class ProcessManager:
self.shared_command_lock.release()
def getData(self):
- if not self.data_queue.empty():
- self.lastest_data = self.data_queue.get_nowait()
- return copy.deepcopy(self.lastest_data)
+ if self.comm_direction != 3:
+ if not self.data_queue.empty():
+ self.latest_data = self.data_queue.get_nowait()
+ if self.comm_direction == 3:
+ self.shared_command_lock.acquire()
+ self.latest_data = pickle.loads(self.shm_data.buf)
+ self.shared_command_lock.release()
+ return copy.deepcopy(self.latest_data)
def terminateProcess(self):
if self.comm_direction == 3:
- self.shm.close()
- self.shm.unlink()
+ self.shm_cmd.close()
+ self.shm_cmd.unlink()
if self.args.debug_prints:
print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
self.command_queue.put_nowait("befree")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 078883ce4c0cb408dd1754e43c241ffae5e1dad8..d0a17c5b87a81e5fe305179912ed06bf62197bb5 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -221,7 +221,7 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
T_w_e = robot.getT_w_e()
#p = T_w_e.translation[:2]
p = q[:2]
- print("CTRL:", p)
+ #print("CTRL:", p)
# NOTE: this is the actual position, not what the path suggested
# whether this or path reference should be put in is debateable.
# this feels more correct to me.
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 38eeaf2a88450b36b2a2e53248091e51a1725ea7..a93d6d84bc2ae6a3181310847a53d17edf420254 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -10,6 +10,7 @@ from ur_simple_control.path_generation.starworlds.obstacles import StarshapedPol
import shapely
import yaml
import pinocchio as pin
+import pickle
import matplotlib.pyplot as plt
import matplotlib.collections as plt_col
@@ -721,7 +722,7 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
# spitting out path points
#def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
# let's try shared memory
-def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, data_queue : Queue):
+def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, shm_data):
"""
starPlanner
------------
@@ -778,8 +779,13 @@ def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, data_queue : Queue)
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
# TODO: this is stupid, just used shared memory bro
- if data_queue.qsize() < 1:
- data_queue.put((path_pol, path_gen.target_path))
+ #if data_queue.qsize() < 1:
+ #data_queue.put((path_pol, path_gen.target_path))
+ data_pickled = pickle.dumps((path_pol, path_gen.target_path))
+ lock.acquire()
+ shm_data.buf[:len(data_pickled)] = data_pickled
+ #shm_data.buf[len(data_pickled):] = bytes(0)
+ lock.release()
except KeyboardInterrupt:
shm.close()