Request for rqt_graph in reconstruction simulation

[JohannaXie]

The planner cannot work properly and the drone seems cannot collect images. The screenshots are given below.

I have checked the code and rqt graph. It is abnormal that the recon.launch file launches three ros nodes: 'waypoint_generator', 'traj_server', and 'exploration_node', while in my rqt_graph, I could only find the 'waypoint_generator' is active. I would like to request your rqt_graph while reconstruction to debug the topics and node states.

I am looking forward to your reply.

The output of the recon.launch file is also given below.

ROS_MASTER_URI=http://localhost:11311

process[exploration_node-1]: started with pid [1342618]
process[traj_server-2]: started with pid [1342619]
process[waypoint_generator-3]: started with pid [1342620]
[ WARN] [1,706,167,787.444,030,751]: [Traj server]: init...
hit: 0.619039, miss: -0.619039, min: -1.99243, max: 2.19722, thresh: 1.38629
0
0
origin_:    -1 -22.5    -1
map size: 45 45 23
hit: 0.619039, miss: -0.619039, min: -1.99243, max: 2.19722, thresh: 1.38629
origin_:    -1 -22.5    -1
map size: 45 45 23
origin_:    -1 -22.5    -1
map size: 45 45 23
yaw diff: 0.523599
max yaw diff: 0.174533
vert num: 5
top:        0
 0.84659
0.532245
bottom:        0
-0.84659
0.532245
left: 0.769831
       0
0.638248
right: -0.771876
        0
 0.635773
lefttop: -3.73084  -2.3951      4.5
leftbottom: -2.87212   2.3951      4.5
righttop: 2.86098 -2.3951     4.5
rightbottom: 2.86098  2.3951     4.5
T_cb:  0 -1  0  0
 0  0  1  0
 1  0  0  0
 0  0  0  1
T_bc: -0 -0  1  0
-1 -0  0 -0
-0  1 -0  0
 0 -0  0  1
Planner Ready!
CUDA ready!
Model loaded!
Warm-Up Started!
Warm-Up Finished!
hit: 0.619039, miss: -0.619039, min: -1.99243, max: 2.19722, thresh: 1.38629
origin_:    -1 -22.5    -1
map size: 45 45 23
origin_:    -1 -22.5    -1
map size: 45 45 23
yaw diff: 0.523599
max yaw diff: 0.174533
vert num: 5
top:        0
 0.84659
0.532245
bottom:        0
-0.84659
0.532245
left: 0.769831
       0
0.638248
right: -0.771876
        0
 0.635773
lefttop: -3.73084  -2.3951      4.5
leftbottom: -2.87212   2.3951      4.5
righttop: 2.86098 -2.3951     4.5
rightbottom: 2.86098  2.3951     4.5
T_cb:  0 -1  0  0
 0  0  1  0
 1  0  0  0
 0  0  0  1
T_bc: -0 -0  1  0
-1 -0  0 -0
-0  1 -0  0
 0 -0  0  1
origin_:    -1 -22.5    -1
map size: 45 45 23
[ INFO] [1,706,167,801.119,477,295]: [FSM]: state: INITIAL
[ WARN] [1,706,167,801.120,682,832]: no odom.
[ INFO] [1,706,167,802.406,008,194]: [FSM]: state: INITIAL
[FSM]: from INITIAL to WAIT
[ INFO] [1,706,167,803.911,381,674]: [FSM]: state: WAIT
[ WARN] [1,706,167,803.911,563,226]: wait for trigger.
[ INFO] [1,706,167,805.104,356,689]: [FSM]: state: WAIT
[ WARN] [1,706,167,805.104,549,715]: wait for trigger.
[ INFO] [1,706,167,806.496,053,764]: [FSM]: state: WAIT
[ WARN] [1,706,167,806.496,101,466]: wait for trigger.
Triggered!
[triggerCallback]: from WAIT to PLAN
[ INFO] [1,706,167,807.534,851,277]: [FSM]: state: PLAN
[ WARN] [1,706,167,807.535,202,674]: STATIC!
start_pt_x:0
start_pt_y:0
start_pt_z:1.53138
-----Planning Start!-----
-----Surface Prediction Start!-----
infer_ms:1.49482ms
ts_ms:5.78514ms
vec_ms:0.016234ms
[ WARN] [1,706,167,807.544,806,947]: near_ground
927
[ WARN] [1,706,167,807.693,939,832]: [Traj server]: ready.
[ WARN] [1,706,167,807.716,315,244]: Visit Cloud!
visit_num:6988
0.200179
Surface_Prediction_Time:180.919ms!
-----Surface Prediction Finished!-----
-----Global Planning Start!-----
preprocess_time:0.100306ms
-----------cluster_size:18
CEC_time:20.7482ms
visibility_time:0.038641ms
cloud number:22985
update_time:336.026ms
partition_size:19
normal_select_time:2.12947ms
qualified_partition_size:19
sample_time:14.8858ms

TSP_time:12.2654ms
[ WARN] [1,706,167,808.105,971,600]: global nbv yaw!
2.53668deg.
total_global:386.194ms
[ WARN] [1,706,167,808.106,220,947]: Global to NBV!
dist to NBV:6.9341
Global_Planning_Time:389.903ms!
-----Global Planning Finished!-----
-----Local Planning Start!-----
None qualified viewpoints...
i:1

None qualified viewpoints...
---Construct Graph---
---Search optimal sequence---
Node: 4, edge: 3
[ WARN] [1,706,167,808.110,349,099]: Same pos!
---Search Finish---
---Push Finish---
-----Local Planning Finished!-----
Local_Planning_Time:4.12948ms!
initial_local_path_length:7.03166m
initial_stoe:6.9341
[ERROR] [1,706,167,808.165,029,674]: Real Length!
real length:7.62633
start to end:6.9341
duration: 19.0658, seg_num: 24, dt: 0.794409
Iter num: 81, time: 0.00513926, point num: 27, comb time: 0.000289158
dt_yaw: 0.791822, start yaw: 7.08578e-07           0           0, end: 0.0442733
Traj_Opt_Time:60.7009ms!
Planning_Time:637.65ms!
-----Planning Finished!-----
traj_id:1_id!
[FSM]: from PLAN to EXEC
[ WARN] [1,706,167,808.526,575,489]: EXEC traj!
[ INFO] [1,706,167,810.230,825,526]: [FSM]: state: EXEC
[ INFO] [1,706,167,811.834,464,011]: [FSM]: state: EXEC
[FSM]: from EXEC to PLAN
[ WARN] [1,706,167,811.834,711,704]: Replan: periodic call=======================================
[ INFO] [1,706,167,813.378,635,441]: [FSM]: state: PLAN
-----Planning Start!-----
-----Surface Prediction Start!-----
infer_ms:1.56825ms
ts_ms:9.03769ms
vec_ms:0.010923ms
[ WARN] [1,706,167,813.392,073,819]: near_ground
918
[ WARN] [1,706,167,813.515,959,218]: Visit Cloud!
visit_num:3631
0.200179
Surface_Prediction_Time:137.763ms!
-----Surface Prediction Finished!-----
-----Global Planning Start!-----
preprocess_time:0.153066ms
-----------cluster_size:18
CEC_time:7.50655ms
visibility_time:0.067381ms
cloud number:22976
update_time:244.347ms
No:16no normal mech!!!!!!!!!!!!
partition_size:18
normal_select_time:7.56886ms
qualified_partition_size:18
sample_time:45.5496ms

TSP_time:16.3766ms
[ WARN] [1,706,167,813.848,144,019]: global nbv yaw!
-58.478deg.
total_global:321.569ms
[ WARN] [1,706,167,813.848,248,959]: Global to NBV!
dist to NBV:11.9875
Global_Planning_Time:331.558ms!
-----Global Planning Finished!-----
-----Local Planning Start!-----
None qualified viewpoints...
i:1

None qualified viewpoints...
---Construct Graph---
---Search optimal sequence---
Node: 4, edge: 3
[ WARN] [1,706,167,813.850,703,933]: Same pos!
---Search Finish---
---Push Finish---
-----Local Planning Finished!-----
Local_Planning_Time:2.47844ms!
[ WARN] [1,706,167,813.850,830,468]: Far Goal!
initial_local_path_length:12.6697m
initial_stoe:11.9875
[ERROR] [1,706,167,813.960,032,009]: Real Length!
real length:13.9204
start to end:11.9875
duration: 34.8011, seg_num: 41, dt: 0.848808
Iter num: 37, time: 0.00133095, point num: 44, comb time: 0.0003582
dt_yaw: 1.82711, start yaw:   0.0163976  0.00511843 0.000783172, end: -1.02063
Traj_Opt_Time:115.239ms!
Planning_Time:587.354ms!
-----Planning Finished!-----
traj_id:2_id!
[FSM]: from PLAN to EXEC
[ WARN] [1,706,167,814.305,285,448]: EXEC traj!
[ INFO] [1,706,167,816.28,990,503]: [FSM]: state: EXEC
[ INFO] [1,706,167,817.286,044,465]: [FSM]: state: EXEC
[ INFO] [1,706,167,818.368,485,870]: [FSM]: state: EXEC
[FSM]: from EXEC to PLAN
[ WARN] [1,706,167,818.368,788,995]: Replan: periodic call=======================================
[ INFO] [1,706,167,819.917,880,637]: [FSM]: state: PLAN
-----Planning Start!-----
-----Surface Prediction Start!-----
infer_ms:1.5188ms
ts_ms:6.6689ms
vec_ms:0.012297ms
[ WARN] [1,706,167,819.927,994,301]: near_ground
918
[ WARN] [1,706,167,819.928,316,318]: Prediction Diff
0
[ WARN] [1,706,167,820.24,147,786]: Visit Cloud!
visit_num:3629
0.200179
Surface_Prediction_Time:106.403ms!
-----Surface Prediction Finished!-----
-----Global Planning Start!-----
preprocess_time:0.126654ms
-----------cluster_size:18
CEC_time:3.45084ms
visibility_time:0.120922ms
cloud number:22976
update_time:153.819ms
No:16no normal mech!!!!!!!!!!!!
partition_size:18
normal_select_time:0.81608ms
qualified_partition_size:18
sample_time:13.4497ms

Originally posted by @JohannaXie in #7 (comment)

[Chen-Albert-FENG]

Sorry for the late reply. I'm very sorry I uploaded the wrong configuration file for Rviz earlier. Now I have fixed it. However, I found your drone did not execute an initial flight, which is manually given in traj_server. You can check the init_test.launch for checking the traj_server node?

[JohannaXie]

I have checked your updates and managed to run your code. I found two typos in your repo.
Firstly, you put the updated traj.rviz config file in /exploration_manager/config/, while the roslaunch exploration_manager rviz.launch launches the traj.rviz in plan_manage package.
Secondly, I found that the libtorch1.12.0-cu116 has some strange conflicts with ros-noetic. I change the libtorch version to 1.12.1-cu116 and that works well.
After I made these two modifications, the code runs pretty well. Thanks for your contribution.

[Chen-Albert-FENG]

Thank you for your rectifications! I will update this repo soon.