Experiments
Preparation
roscorerosrun pcl_ros pcd_to_pointcloud ~/.ros/wh2_lg707070_1ms0.01_filtered.pcd _frame_id:=/map
In the directory of ~/truss_ws/src/truss
rviz -d node_pole.rviz
In another terminal, open rviz -> Panels -> Selection. Choose Select, then left click the point whose position you want to know. On the left Selection you can see it.
The point on the pole it's position is:
// X Y Z0.2518590.0151518-0.0633736
Modify values of RES and init_x, init_y and init_z of the truss_pointcloud.cpp and skeleton_truss.cpp in the directory of /home/william/truss_ws/src/truss/src
In truss_pointcloud.cpp, it seems like:
#define RES 100 //occmap resulotion, number of cell per meter(one demention)
init_x=0.251859; init_y=-0.251859; init_z=-0.251859;
In skeleton_truss.cpp, it seems like:
#define RES 100 //occmap resulotion, number of cell per meter(one demention)
float init_x=0.251859,init_y=-0.251859,init_z=-0.251859; // must be in the truss
Successful demo
init_x=0.12; init_y=0.12; init_z=0.526;
Run node truss_pointcloud.
rosrun truss truss_pointcloud
Publish pointcloud
rosrun pcl_ros pcd_to_pointcloud /home/william/Documents/demos/cylinders/preprocessing/data/realSense02.pcd 10 _frame_id:=/map
output:
size 92 80 122[ INFO] [1555152658.163952435]: n=137,DETECT_time=0,tot=10.46[ INFO] [1555152658.235489374]: n=350,DETECT_time=1,tot=11.89[ INFO] [1555152658.365974220]: n=734,DETECT_time=2,tot=15.27[ INFO] [1555152658.451850002]: n=991,DETECT_time=3,tot=15.29[ INFO] [1555152658.854427484]: n=2179,DETECT_time=4,tot=12.83[ INFO] [1555152659.265372521]: n=3354,DETECT_time=5,tot=12.42[ INFO] [1555152659.665302454]: n=4504,DETECT_time=6,tot=26.82[ INFO] [1555152660.201265043]: n=6039,DETECT_time=7,tot=10.18[ INFO] [1555152660.736159441]: collect time=678,rollback time=178[ INFO] [1555152660.738436210]: Finish
Run node skeleton_truss
rosrun truss skeleton_truss
output:
[ INFO] [1555153870.996387804]: Skeleton start[ INFO] [1555153911.036309147]: collect time=20[ INFO] [1555153911.036339322]: Skeleton finish
Run node node_pole
rosrun truss node_pole
Output:
vector[1].x=0.02,y=0.12,z=0.00,size of vector[3]=77vector[2].x=0.00,y=0.08,z=-0.04,size of vector[4]=26vector[3].x=-0.02,y=0.00,z=0.00,size of vector[6]=97vector[4].x=0.00,y=-0.02,z=0.02,size of vector[17]=30vector[5].x=0.00,y=0.00,z=-0.02,size of vector[19]=46vector[6].x=-0.04,y=0.02,z=0.00,size of vector[29]=24vector[7].x=0.04,y=-0.04,z=0.00,size of vector[73]=24[ INFO] [1555153941.073078888]: Type of vector=7[ INFO] [1555153941.073190788]: Number of original pole=39[ INFO] [1555153941.073590573]: Number of true poleos=8[ INFO] [1555153941.826587964]: Pole 0: radius=0.07;end1.x=0.12,end1.y=-0.29,end1.z=0.64;end2.x=-0.09,end2.y=0.09,end2.z=0.68[ INFO] [1555153941.826616263]: polenumber=2
Then Fixd Frame select map, see topic /pcl_node in rviz
Question:
- Does the position of the point on the pole will be affected whether it is near the background?
- Valify it by choosing another point it is not near by the background.
observe the scenario, noise exist near the pole, maybe should tune down the search distance.
modify truss_pointcloud.cpp line 354, init_x, init_y, init_z
save settings of rviz in node_pole.
rviz -d node_pole
In order to run node truss pointCloud once, we will run it at first. Then publish pointcloud only one time.
Update in 20190528
Tune down the values of RANGE and ADDLI when the truss is nearer to the background.
#define DIS 8 //Fisrt filtering#define RES 100 //occmap resulotion, number of cell per meter(one demention)// #define RES 500 //occmap resulotion, number of cell per meter(one demention)#define SDEP 200 //depth of search, size of close list#define RANGE 5 //range of initial search, rely on the resulotion of pointcloud #define ADDLI 3 //limit of additional search range, rely on the Climbot max #define ERROR 2 //allowable error#define POSTEP 3000 //max step of search.#define LOCALDIS 0.1 //distance for identify the local optima seperately#define ALLOWDUP 2 //when fall into local optima, times to duplicate and escape#define COLLECT 3000 //Sample time#define DESCRITH 10
The original pointcloud:
Picture after truss_pointcloud
#define RES 100 //occmap resulotion, number of cell per meter(one demention)#define SDEP 100 //depth of search, size of close list#define RANGE 6 //range of initial search, rely on the max step length of Climbot#define ADDLI 4 //limit of additional search range, to increace the search range when needed#define ERROR 8 //allowable error, cell#define POSTEP 300 //max step of forward search.#define OPDIR 100 //max step of leaving search.#define LOCALDIS 0.1 //distance for identify the local optima seperately#define ALLOWDUP 2 //when fall into local optima, times to duplicate and escape
#define SAMPLE_TH 10 //vector size threshold 24#define ANGLE 27 //vector angle range 27#define POLE_LENGTH 4 //pole length threshold#define POLE_DIS 0.5 //repeated poles check distance#define SHOW_POLE 99 //99 means all the true poles, 999 means all the poles#define OBB_SIZE 0.08 //the size of expand ball from one point#define PP_SKIP 4 //the allowed breakup of pointcloud along one pole
[ INFO] [1559107770.253840402]: Pole 1: radius=0.08;end1.x=-0.65,end1.y=-0.69,end1.z=-0.31;end2.x=-0.58,end2.y=-0.75,end2.z=0.34[ INFO] [1559107770.253870503]: Pole 5: radius=0.08;end1.x=0.33,end1.y=0.02,end1.z=0.39;end2.x=0.40,end2.y=-0.09,end2.z=1.02[ INFO] [1559107770.253906031]: Pole 12: radius=0.08;end1.x=-0.35,end1.y=0.77,end1.z=-0.14;end2.x=-1.03,end2.y=-0.06,end2.z=0.49[ INFO] [1559107770.253920717]: Pole 15: radius=0.08;end1.x=0.37,end1.y=0.01,end1.z=0.18;end2.x=-0.32,end2.y=0.78,end2.z=0.43[ INFO] [1559107770.253934686]: Pole 17: radius=0.07;end1.x=0.40,end1.y=-0.13,end1.z=0.98;end2.x=-0.18,end2.y=0.44,end2.z=0.71[ INFO] [1559107770.253947612]: Pole 18: radius=0.05;end1.x=-0.81,end1.y=0.01,end1.z=0.51;end2.x=-0.69,end2.y=-0.64,end2.z=-0.16[ INFO] [1559107770.253957812]: Pole 19: radius=0.06;end1.x=0.00,end1.y=-0.32,end1.z=0.36;end2.x=-0.47,end2.y=-0.80,end2.z=0.91[ INFO] [1559107770.253969030]: Pole 20: radius=0.08;end1.x=-0.35,end1.y=0.77,end1.z=-0.14;end2.x=-1.03,end2.y=-0.06,end2.z=0.49[ INFO] [1559107770.253980318]: Pole 21: radius=0.08;end1.x=-0.33,end1.y=0.82,end1.z=-0.21;end2.x=-0.26,end2.y=0.66,end2.z=0.35[ INFO] [1559107770.253996517]: Pole 22: radius=0.07;end1.x=-0.00,end1.y=-0.35,end1.z=0.35;end2.x=-0.47,end2.y=-0.80,end2.z=0.40[ INFO] [1559107770.254006475]: Pole 23: radius=0.08;end1.x=-0.53,end1.y=-0.87,end1.z=0.87;end2.x=-0.47,end2.y=-0.75,end2.z=0.96[ INFO] [1559107770.254018620]: polenumber=11
Publish topics
/allpath/allpath2/cube/detectpoints/envelopeline/ldetectpoints/openvelopeline/output2_pcl/pcl_node
truss_pointcloud
/allpath/output2_pcl
skeleton_truss
/allpath2
node_pole
/envelopeline/openvelopeline/pcl_node