-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathROS2 command line examples for SmartTemplate robot.txt
245 lines (191 loc) · 9.06 KB
/
ROS2 command line examples for SmartTemplate robot.txt
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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
ROS2 command line for SmartTemplate (3DOF)
TO BUILD SYSTEM PACKAGES:
colcon build --cmake-args -DOpenIGTLink_DIR:PATH=/home/smartneedle/OpenIGTLink-build
TO USE DEBUG MODE:
--ros-args --log-level debug
TO LAUNCH ROBOT
*Launch SmartTemplate packages
ros2 launch smart_template robot.launch.py sim_level:=1 gui:=true rviz:=true
OBS: all arguments are optional
sim_level (default = 2): 1=virtual / 2=real (chooses if real robot or simulated)
gui (default = true): true/false (load rqt plugin with user gui for commanding robot)
rviz (default = false): true/false (load rviz for robot visualization)
_______________________________________________________________________________________
1) Launch insertion demo (stepwise openloop insertion - NO needle feedback)
WHAT IT DOES?
This demo receives the target and entry point (in ZFrame coordinates) through OpenIGTLink communication,
approaches the skin, and then perform step-wise insertions (no lateral motion, only insertion) according to the plan.
It does not have needle tip feedback, assumes the needle performs perfect straight trajectory
***ROS2 computer***
*Launch igtl_bridge server to 3DSlicer
ros2 launch ros2_igtl_bridge bridge.launch.py
<CONFIGURE 3DSLICER NOW - see bellow>
*Launch smart_template packages
ros2 launch smart_template robot.launch.py gui:=true sim_level:=2
ros2 run smart_template keypress
Use SPACE for next insertion step
Use ENTER to finalize insertion procedure
*Launch trajcontrol packages
ros2 launch trajcontrol step_openloop.launch.py
***3DSlicer***
Open Slicer (5.7 version compiled with local OpenSSL)
Step 1: ZFrameRegistrationROI module (IGT)
*Get zFrame registration
Step 2: Open SmartNeedle module (IGT)
*Set zFrameTransform
*Set IP and Port and start client
*Select TARGET and ENTRY points
<WAIT>
Step 3: When requested by ROS, hit "Send planned points"
<PERFORMS INSERTION PROCEDURE according to the controller_step_openloop terminal instructions>
Step 4: At end of insertion, hit "Save copy" to store a copy of the nodes
_______________________________________________________________________________________
1) Launch robot in manual mode - DEPRECATED
*Launch SmartTemplate packages
ros2 launch smart_template robot.launch.py sim_level:=1
ros2 run smart_template keypress
*Launch trajcontrol packages
ros2 launch trajcontrol manual_smart.launch.py insertion_step:=10.0 lateral_step:=1.0
OBS:
sim_level argument (default = 2) is optional: 1=virtual / 2=real
insertion_step (default = 10.0 mm) and lateral_step (default = 1.0 mm) arguments are optional
Use numkeyboard arrows to move robot in horizontal and vertical directions.
Use ENTER for needle insertion and SPACE for needle retraction.
Use R to retract the needle, H for homing the robot, and A to abort current motion (to be implemented in the robot).
_______________________________________________________________________________________
3) Initialize Jacobian (uses rand controller)
***ROS2 computer***
*Launch SmartNeedle packages
ros2 launch trajcontrol jhu_needle.launch.py
ros2 run hyperion_interrogator calibrate_sensors --ros-args -r __ns:=/needle
*Launch SmartTemplate packages
ros2 launch smart_template robot.launch.py sim_level:=2
ros2 run smart_template keypress
*Launch trajcontrol packages
ros2 launch trajcontrol smartneedle_init_jacobian.launch.py | tee -a initJ.log
OBS:
- air_gap argument is optional (default=25.0mm)
- insertion_step argument is optional (default=5.0mm)
- total_steps argument is optional (default=4)
- Use SPACE to signal a new step.
_______________________________________________________________________________________
4) Launch Smart Needle experiment (Open-loop control)
***ROS2 computer***
*Launch SmartNeedle packages
ros2 launch trajcontrol jhu_needle.launch.py
ros2 run hyperion_interrogator calibrate_sensors --ros-args -r __ns:=/needle
*Launch igtl_bridge server to 3DSlicer
ros2 launch ros2_igtl_bridge bridge.launch.py
<CONFIGURE 3DSLICER NOW - see bellow>
*Launch SmartTemplate packages
ros2 launch smart_template robot.launch.py sim_level:=2
ros2 run smart_template keypress
*Launch trajcontrol packages
ros2 launch trajcontrol smartneedle_openloop.launch.py num_blocks:=0 insertion_step:=5.0 filename:=openloop_1 | tee -a openloop_1.log
=> Follow terminal instructions
OBS:
- use_slicer argument is optional (default=False)
- insertion_length argument is optional when use_slicer is False (default=10.0).
When use_slicer=True, insertion_length is set by target
- Use numkeyboard arrows to move robot in horizontal and vertical directions.
- Use ENTER for needle insertion and SPACE for needle retraction.
***3DSlicer***
Open Slicer (using version 5.3)
Step 1: ZFrameRegistrationROI module (IGT)
*Get zFrame registration
Step 2: Open SmartNeedle module (IGT)
*Set zFrameTransform
*Set IP and Port and start client
*Select TARGET and ENTRY points
<WAIT>
Step 3: When requested by ROS, hit "Send planned points"
Step 4: At end of insertion, hit "Save copy" to store a copy of the nodes
_______________________________________________________________________________________
5) Launch Smart Needle experiment (MPC control)
***ROS2 computer***
*Launch SmartNeedle packages
ros2 launch trajcontrol jhu_needle.launch.py
ros2 run hyperion_interrogator calibrate_sensors --ros-args -r __ns:=/needle
*Launch igtl_bridge server to 3DSlicer
ros2 launch ros2_igtl_bridge bridge.launch.py
<CONFIGURE 3DSLICER NOW - see bellow>
*Launch SmartTemplate packages
ros2 launch smart_template robot.launch.py sim_level:=2
ros2 run smart_template keypress
*Launch trajcontrol packages
ros2 launch trajcontrol smartneedle_mpc.launch.py num_blocks:=0 insertion_step:=5.0 filename:=mpc_1 | tee -a mpc_1.log
=> Follow terminal instructions
OBS:
- use_slicer argument is optional (default=False)
- insertion_length argument is optional when use_slicer is False (default=10.0).
When use_slicer=True, insertion_length is set by target
- Use numkeyboard arrows to move robot in horizontal and vertical directions.
- Use ENTER for needle insertion and SPACE for needle retraction.
***3DSlicer***
Open Slicer (using version 5.3)
Step 1: ZFrameRegistrationROI module (IGT)
*Get zFrame registration
Step 2: Open SmartNeedle module (IGT)
*Set zFrameTransform
*Set IP and Port and start client
*Select TARGET and ENTRY points
<WAIT>
Step 3: When requested by ROS, hit "Send planned points"
Step 4: At end of insertion, hit "Save copy" to store a copy of the nodes
_______________________________________________________________________________________
6) Launch SmartTemplate validation (Cube control)
***ROS2 computer***
*Launch igtl_bridge server to 3DSlicer
ros2 launch ros2_igtl_bridge bridge.launch.py
<CONFIGURE 3DSLICER NOW - see bellow>
*Launch SmartTemplate packages
ros2 launch smart_template robot.launch.py sim_level:=2
ros2 run smart_template keypress
*Launch trajcontrol packages
ros2 launch trajcontrol smartneedle_cube.launch.py cube_size:=30.0 fiducial_offset:=0.0 filename:=cube_AMIGO_1 | tee -a cube_AMIGO_1.log
=> Follow terminal instructions
OBS:
- cube_size argument is optional (default=50.0)
- fiducial_offset argument is optional (default=20.0)
- filename argument is optional (default='sequence')
- Use numkeyboard arrows to move robot in horizontal and vertical directions.
- Use ENTER for needle insertion and SPACE for needle retraction.
***3DSlicer***
Open Slicer (using version 5.3)
Step 1: ZFrameRegistrationROI module (IGT)
*Get zFrame registration
Step 2: Open SmartNeedle module (IGT)
*Set zFrameTransform
*Set IP and Port and start client
*Select TARGET and ENTRY points to be the same and equal do the desired cube center
<WAIT>
Step 3: When requested by ROS, hit "Send planned points"
_______________________________________________________________________________________
EXTRAS:
A) Bag files:
ros2 bag record -a -o BAG_NAME
ros2 bag info BAG_NAME
B) Check interrogator topics:
ros2 topic list
ros2 topic echo topic_name
ros2 topic hz topic_name
C) Shutdown scripts (pkill of all launched nodes):
For needle: ./needle_kill.sh
For control: ./control_kill.sh
For robot: ./robot_kill.sh
D) Kill zoombie nodes (Example with igtl_bridge server)
pkill -f -9 igtl_bridge
OR
ps -aux | grep igtl_bridge
kill -9 NUMBER
E) Save terminal log:
ros2 launch trajcontrol smartneedle_openloop.launch.py | tee -a openloop.log
F) SmartTemplate Action and Services:
ros2 action send_goal /stage/move smart_template_interfaces/action/MoveStage "{x: 10.0, 7: 0.0, z: 0.0, eps: 0.05}"
ros2 service call /stage/command smart_template_interfaces/srv/ControllerCommand "command: HOME"
ros2 service call /stage/command smart_template_interfaces/srv/ControllerCommand "command: RETRACT"
ros2 service call /stage/command smart_template_interfaces/srv/ControllerCommand "command: ABORT"
From inside DICOM folder:
sudo sh -c "export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/smartneedle/Slicer-5.4.0-linux-amd64/lib/Slicer-5.4; /home/smartneedle/Slicer-5.4.0-linux-amd64/bin/storescp -v 104"
Dell computer:
sudo sh -c "export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/Slicer5.3.0/lib/Slicer-5.3; /opt/Slicer5.3.0/bin/storescp -v 104"