Skip to content

Commit 74f89cd

Browse files
nikhil-sethinikhil-sethi
committed
Expose tasks via CMake
1 parent 3f069b2 commit 74f89cd

File tree

3 files changed

+347
-11
lines changed

3 files changed

+347
-11
lines changed

‎snp_motion_planning/CMakeLists.txt‎

Lines changed: 19 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
99
add_compile_options(-Wall -Wextra -Wpedantic)
1010
endif()
1111

12+
include_directories(
13+
include
14+
)
15+
1216
set(ROS2_DEPS
1317
rclcpp
1418
snp_msgs
@@ -70,18 +74,9 @@ target_link_libraries(
7074
tesseract::tesseract_kinematics_kdl
7175
yaml-cpp)
7276

73-
# Plugin Library
74-
add_library(${PROJECT_NAME}_plugins SHARED src/plugins/kinematics/motoman_hc10_ikfast_plugin.cpp)
75-
target_link_libraries(${PROJECT_NAME}_plugins PUBLIC ${LAPACK_LIBRARIES} tesseract::tesseract_common
76-
tesseract::tesseract_kinematics_ikfast)
77-
target_include_directories(${PROJECT_NAME}_plugins SYSTEM PUBLIC ${LAPACK_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
78-
7977
# Install the executable(s)
8078
install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
8179

82-
# Install the library(ies)
83-
install(TARGETS ${PROJECT_NAME}_plugins ${PROJECT_NAME}_tasks EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
84-
8580
# Install Python modules
8681
ament_python_install_package(${PROJECT_NAME})
8782
# Install Python executables
@@ -91,14 +86,27 @@ install(PROGRAMS
9186
DESTINATION lib/${PROJECT_NAME}
9287
)
9388

89+
install(DIRECTORY include/${PROJECT_NAME} DESTINATION include)
90+
9491

9592
if(BUILD_TESTING)
9693
find_package(ament_lint_auto REQUIRED)
9794
ament_lint_auto_find_test_dependencies()
9895
endif()
9996

97+
ament_export_include_directories(include)
10098
ament_export_dependencies(${ROS2_DEPS})
101-
ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
102-
ament_package()
99+
ament_export_targets(${PROJECT_NAME}_tasks HAS_LIBRARY_TARGET)
103100

104101
install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}/)
102+
103+
install(
104+
TARGETS ${PROJECT_NAME}_tasks
105+
EXPORT ${PROJECT_NAME}_tasks
106+
LIBRARY DESTINATION lib
107+
ARCHIVE DESTINATION lib
108+
RUNTIME DESTINATION bin
109+
INCLUDES DESTINATION include
110+
111+
)
112+
ament_package()

‎snp_motion_planning/config/convert_toolpath.py‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
import yaml
2+
3+
def convert_back_pose(pose_dict):
4+
return {
5+
'position': {
6+
'x': pose_dict['x'],
7+
'y': pose_dict['y'],
8+
'z': pose_dict['z']
9+
},
10+
'orientation': {
11+
'x': pose_dict['qx'],
12+
'y': pose_dict['qy'],
13+
'z': pose_dict['qz'],
14+
'w': pose_dict['qw']
15+
}
16+
}
17+
18+
def convert_nested_to_task_yaml(input_yaml):
19+
with open(input_yaml, 'r') as file:
20+
nested_data = yaml.safe_load(file)
21+
22+
task_allocation = []
23+
24+
for i, task_group in enumerate(nested_data): # Each task
25+
task_entry = {
26+
'name': f'task_entry_motion_{i}',
27+
'sensors': {
28+
'name': '',
29+
'mode': 'OFF'
30+
},
31+
'manipulation': []
32+
}
33+
print(len(task_group))
34+
for j, segment_poses in enumerate(task_group): # Each segment in the task
35+
segment = {
36+
'name': f'segment_{j + i * 10}', # Or use just j if you prefer
37+
'poses': []
38+
}
39+
40+
for poses in segment_poses:
41+
for pose in poses:
42+
segment['poses'].append(convert_back_pose(pose))
43+
44+
task_entry['manipulation'].append(segment)
45+
46+
task_allocation.append(task_entry)
47+
48+
task_structure = {
49+
'task': {
50+
'super_state': 'ndtInspection',
51+
'task_allocation': task_allocation
52+
}
53+
}
54+
55+
return task_structure
56+
57+
def save_as_yaml(data, output_yaml):
58+
with open(output_yaml, 'w') as file:
59+
yaml.dump(data, file, default_flow_style=False)
60+
61+
if __name__ == "__main__":
62+
input_yaml = '/workspace/src/scan_n_plan_workshop/snp_motion_planning/config/toolpath.yaml' # Your nested pose format
63+
output_yaml = '/workspace/src/nxtgen_inspection_sm/sm_mobile_ndt/config/jobs/task_ndt_composite_speed.yaml'
64+
65+
task_yaml = convert_nested_to_task_yaml(input_yaml)
66+
save_as_yaml(task_yaml, output_yaml)
67+
print(f"Reconstructed YAML written to: {output_yaml}")

‎snp_motion_planning/config/toolpath_sm.yaml‎

Lines changed: 261 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,261 @@
1+
-
2+
-
3+
-
4+
- qw: 0.004986713031622316
5+
qx: -0.07793066912874154
6+
qy: 0.0007846599502398713
7+
qz: 0.9969460004488957
8+
x: 0.0004842154448851943
9+
y: -1.4100733995437622
10+
z: -0.0025229386519640684
11+
- qw: 0.004986713031622316
12+
qx: -0.07793066912874154
13+
qy: 0.0007846599502398713
14+
qz: 0.9969460004488957
15+
x: 0.019951604306697845
16+
y: -1.4102667570114136
17+
z: 0.0007849433459341526
18+
- qw: 0.005002693023618797
19+
qx: -0.06747192389390057
20+
qy: 0.0009063329664144652
21+
qz: 0.9977082194254596
22+
x: 0.05743969976902008
23+
y: -1.4106395244598389
24+
z: 0.00613534078001976
25+
- qw: 0.005031101939848325
26+
qx: -0.046655029273608664
27+
qy: 0.0011689708477119899
28+
qz: 0.9988977073574185
29+
x: 0.09562046825885773
30+
y: -1.4110198020935059
31+
z: 0.010254361666738987
32+
- qw: 0.005041668621051365
33+
qx: -0.026901476394831963
34+
qy: 0.001420398373742254
35+
qz: 0.9996243667567101
36+
x: 0.13392628729343414
37+
y: -1.4114017486572266
38+
z: 0.013034529983997345
39+
- qw: 0.0050386466033920715
40+
qx: -0.005401685271853137
41+
qy: 0.0016185226553705888
42+
qz: 0.9999714067017337
43+
x: 0.17231087386608124
44+
y: -1.4117851257324219
45+
z: 0.014344742521643639
46+
- qw: 0.005018699906871694
47+
qx: 0.013907475132334167
48+
qy: 0.0019085132649452905
49+
qz: 0.9998888700069652
50+
x: 0.21071772277355194
51+
y: -1.4121692180633545
52+
z: 0.01419512927532196
53+
- qw: 0.004992409526971616
54+
qx: 0.03282193846086334
55+
qy: 0.0021798397719904445
56+
qz: 0.9994463689970337
57+
x: 0.2490965723991394
58+
y: -1.4125536680221558
59+
z: 0.012732498347759247
60+
- qw: 0.004929061455938332
61+
qx: 0.04343565764682736
62+
qy: 0.0022648272520990665
63+
qz: 0.9990414999165297
64+
x: 0.28739097714424133
65+
y: -1.4129377603530884
66+
z: 0.009816630743443966
67+
- qw: 0.0048663630770686235
68+
qx: 0.062388584362678266
69+
qy: 0.002550439294381521
70+
qz: 0.9980368121021533
71+
x: 0.32556450366973877
72+
y: -1.4133211374282837
73+
z: 0.005484301131218672
74+
-
75+
-
76+
- qw: 0.00485912355154414
77+
qx: 0.062051867305432
78+
qy: 0.0027590571422973016
79+
qz: 0.998057284070361
80+
x: 0.3254961371421814
81+
y: -1.5033247470855713
82+
z: 0.0059569827280938625
83+
- qw: 0.004922566971446661
84+
qx: 0.04316831078088589
85+
qy: 0.0025463911577979286
86+
qz: 0.9990524416519926
87+
x: 0.2873583137989044
88+
y: -1.5029417276382446
89+
z: 0.010232768952846527
90+
- qw: 0.004992987516118157
91+
qx: 0.03259571683678087
92+
qy: 0.002419578121812722
93+
qz: 0.9994532179953561
94+
x: 0.24905790388584137
95+
y: -1.502557635307312
96+
z: 0.013091550208628178
97+
- qw: 0.005022539898247144
98+
qx: 0.01353662900543489
99+
qy: 0.002208393786305882
100+
qz: 0.9998933226924881
101+
x: 0.21067661046981812
102+
y: -1.5021731853485107
103+
z: 0.01451333798468113
104+
- qw: 0.005046287358145638
105+
qx: -0.005811664428499613
106+
qy: 0.001958947489584137
107+
qz: 0.9999684605352324
108+
x: 0.17226913571357727
109+
y: -1.5017890930175781
110+
z: 0.0146331200376153
111+
- qw: 0.005054447075495158
112+
qx: -0.0271781066605266
113+
qy: 0.0016886680135270245
114+
qz: 0.9996164021680768
115+
x: 0.1338822841644287
116+
y: -1.5014057159423828
117+
z: 0.013343703001737595
118+
- qw: 0.005044904796552793
119+
qx: -0.04710006014565197
120+
qy: 0.0014174403777564384
121+
qz: 0.9988764308625192
122+
x: 0.09557326883077621
123+
y: -1.5010236501693726
124+
z: 0.01056517194956541
125+
- qw: 0.005018695074096544
126+
qx: -0.06791624335460865
127+
qy: 0.0011145471704411024
128+
qz: 0.997677780835554
129+
x: 0.05738726630806923
130+
y: -1.5006434917449951
131+
z: 0.006390026304870844
132+
- qw: 0.005002793308190302
133+
qx: -0.07833504430368805
134+
qy: 0.000961864525434585
135+
qz: 0.9969140723802074
136+
x: 0.019318100064992905
137+
y: -1.5002650022506714
138+
z: 0.0008601610898040235
139+
-
140+
-
141+
- qw: 0.005022003162392893
142+
qx: -0.0790502290025761
143+
qy: 0.0011599248670142723
144+
qz: 0.9968573094245635
145+
x: 0.01978740096092224
146+
y: -1.5902740955352783
147+
z: 0.0012296584900468588
148+
- qw: 0.005033978003359444
149+
qx: -0.06862984919460972
150+
qy: 0.0013578451834967999
151+
qz: 0.9976285677151815
152+
x: 0.057350609451532364
153+
y: -1.5906474590301514
154+
z: 0.006654616445302963
155+
- qw: 0.00506545825503371
156+
qx: -0.04771635327145732
157+
qy: 0.0017099215942290038
158+
qz: 0.998846618320992
159+
x: 0.09552772343158722
160+
y: -1.5910277366638184
161+
z: 0.010854569263756275
162+
- qw: 0.0050689704064942305
163+
qx: -0.02768448584489622
164+
qy: 0.0020004340606250004
165+
qz: 0.9996018572642265
166+
x: 0.13383476436138153
167+
y: -1.591409683227539
168+
z: 0.013692314736545086
169+
- qw: 0.005058332186819972
170+
qx: -0.006271139349755167
171+
qy: 0.002361768677489835
172+
qz: 0.9999647534465692
173+
x: 0.172223299741745
174+
y: -1.5917929410934448
175+
z: 0.015044967643916607
176+
- qw: 0.00502791514328684
177+
qx: 0.013226924682870532
178+
qy: 0.0025598855035921394
179+
qz: 0.9998966024139466
180+
x: 0.21063542366027832
181+
y: -1.592177152633667
182+
z: 0.014948397874832153
183+
- qw: 0.004989425800230573
184+
qx: 0.03225544386420846
185+
qy: 0.002661057353937697
186+
qz: 0.9994636615430627
187+
x: 0.24902109801769257
188+
y: -1.5925614833831787
189+
z: 0.013533267192542553
190+
- qw: 0.0049128686486628265
191+
qx: 0.04277147680212499
192+
qy: 0.0028410978633959904
193+
qz: 0.999068762726939
194+
x: 0.287324458360672
195+
y: -1.5929456949234009
196+
z: 0.010666937567293644
197+
- qw: 0.004848387379244614
198+
qx: 0.06192607496386882
199+
qy: 0.002962422985194974
200+
qz: 0.998064566262848
201+
x: 0.3254973292350769
202+
y: -1.5933290719985962
203+
z: 0.006382694933563471
204+
-
205+
-
206+
- qw: 0.004904082635533626
207+
qx: 0.042503097601205776
208+
qy: 0.0032226915666874107
209+
qz: 0.9990791014363538
210+
x: 0.28729671239852905
211+
y: -1.6829496622085571
212+
z: 0.011193695478141308
213+
- qw: 0.004985916245333362
214+
qx: 0.031881818796215454
215+
qy: 0.0032711133675066132
216+
qz: 0.9994738566299654
217+
x: 0.24898356199264526
218+
y: -1.682565450668335
219+
z: 0.013998677022755146
220+
- qw: 0.005037593694841437
221+
qx: 0.013164688085252188
222+
qy: 0.003149728080618573
223+
qz: 0.9998956909850156
224+
x: 0.21059291064739227
225+
y: -1.6821811199188232
226+
z: 0.015401114709675312
227+
- qw: 0.0050678142726465316
228+
qx: -0.006563160101696104
229+
qy: 0.0027207357557727844
230+
qz: 0.9999619191673874
231+
x: 0.17217697203159332
232+
y: -1.6817967891693115
233+
z: 0.01546697411686182
234+
- qw: 0.005086523821132416
235+
qx: -0.028289372449795312
236+
qy: 0.002402628138681321
237+
qz: 0.9995839464796545
238+
x: 0.13378436863422394
239+
y: -1.6814135313034058
240+
z: 0.014107824303209782
241+
- qw: 0.005088804117300729
242+
qx: -0.04828025419388572
243+
qy: 0.002087891369029567
244+
qz: 0.9988186831638968
245+
x: 0.09547081589698792
246+
y: -1.6810314655303955
247+
z: 0.011273316107690334
248+
- qw: 0.005058368537832509
249+
qx: -0.06934977394608938
250+
qy: 0.001614676500526345
251+
qz: 0.997578274914385
252+
x: 0.05728747323155403
253+
y: -1.6806511878967285
254+
z: 0.0070284404791891575
255+
- qw: 0.0050325972410452365
256+
qx: -0.07979429269959183
257+
qy: 0.001321175447109165
258+
qz: 0.9967977720245061
259+
x: 0.018789276480674744
260+
y: -1.680268406867981
261+
z: 0.0013130001025274396

0 commit comments

Comments
 (0)