SystemIntegrationForManualInsertion/it_works.py at main · SmartNeedle/SystemIntegrationForManualInsertion · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
import slicer
import time

#################### ROS2 Topic Subscription #######################
rosLogic = slicer.util.getModuleLogic('ROS2')
rosNode = rosLogic.GetDefaultROS2Node()

topic_name = '/needle/state/current_shape'
msg_type = 'PoseArray'

# Create subscriber node
subShape = rosNode.CreateAndAddSubscriberNode(msg_type, topic_name)
if subShape is None:
    raise RuntimeError(f"Failed to create subscriber for {topic_name}")
print(f"Subscribed to {topic_name}")

#################### Fiducial and Curve Nodes #######################
# Create fiducial node to store individual points
fid = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLMarkupsFiducialNode", "NeedleHeader")

# Create curve node
cur = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLMarkupsCurveNode", "NeedleShape2")

#### Change
# ------------------ HIDE LABELS HERE ------------------
# Ensure display nodes exist
if fid.GetDisplayNode() is None:
    fid.CreateDefaultDisplayNodes()
if cur.GetDisplayNode() is None:
    cur.CreateDefaultDisplayNodes()

# Hide labels for fiducial points
fid.GetDisplayNode().SetPointLabelsVisibility(False)

# Hide labels for curve control points
cur.GetDisplayNode().SetPointLabelsVisibility(False)
# -------------------------------------------------------
#### End Change

#################### Throttle Setup #######################
THROTTLE_INTERVAL = 2  # seconds
lastUpdateTime = 0

#################### Callback Function #######################
def updateFiducialsAndCurve(caller=None, event=None):
    global lastUpdateTime
    currentTime = time.time()

    # Throttle updates: only allow once every THROTTLE_INTERVAL
    if currentTime - lastUpdateTime < THROTTLE_INTERVAL:
        return
    lastUpdateTime = currentTime

    # Get the latest PoseArray message from ROS
    poseArray = subShape.GetLastMessage()
    if poseArray is None:
        return

    # Extract the individual poses from the message
    poses = poseArray.GetPoses()
    if not poses:
        return

    # Clear existing control points in both fiducial and curve
    fid.RemoveAllControlPoints()
    cur.RemoveAllControlPoints()

    # Add each pose to both fiducial and curve nodes
    for pose in poses:
        x=pose.GetElement(0,3)/1000
        y=pose.GetElement(1,3)/1000
        z=pose.GetElement(2,3)/1000
        fid.AddControlPoint(x,y,z)
        cur.AddControlPoint(x,y,z)
    print("Printed 402 points.")

#################### Add Observer #######################
observerId = subShape.AddObserver('ModifiedEvent', updateFiducialsAndCurve)

print("Throttled receiver set up and ready.")