region id bug fixed. save cloud regardless of the correctness.

cloud_scan3.txt

@@ -1,211 +1,211 @@
-2.746 -1.430 0.000
+2.745 -1.430 0.000
 2.778 -1.426 0.000
-2.816 -1.425 0.000
-2.846 -1.421 0.000
+2.814 -1.424 0.000
+2.845 -1.420 0.000
 2.870 -1.414 0.000
-2.885 -1.404 0.000
-2.899 -1.394 0.000
-2.904 -1.379 0.000
+2.884 -1.404 0.000
+2.896 -1.392 0.000
+2.902 -1.378 0.000
 2.907 -1.364 0.000
 2.908 -1.348 0.000
-2.905 -1.330 0.000
-2.900 -1.310 0.000
-2.892 -1.290 0.000
-2.889 -1.272 0.000
-2.906 -1.263 0.000
-3.039 -1.312 0.000
-3.117 -1.335 0.000
-3.122 -1.322 0.000
-3.117 -1.303 0.000
-3.125 -1.291 0.000
-3.144 -1.285 0.000
-3.135 -1.265 0.000
-3.127 -1.244 0.000
+2.906 -1.330 0.000
+2.899 -1.310 0.000
+2.893 -1.291 0.000
+2.888 -1.271 0.000
+2.909 -1.264 0.000
+3.045 -1.315 0.000
+3.116 -1.334 0.000
+3.124 -1.323 0.000
+3.121 -1.305 0.000
+3.126 -1.292 0.000
+3.146 -1.287 0.000
+3.138 -1.266 0.000
+3.129 -1.245 0.000
 3.135 -1.232 0.000
-3.149 -1.224 0.000
-3.155 -1.211 0.000
-3.137 -1.184 0.000
-3.108 -1.150 0.000
-3.031 -1.087 0.000
-2.905 -0.993 0.000
+3.154 -1.226 0.000
+3.158 -1.213 0.000
+3.143 -1.187 0.000
+3.116 -1.155 0.000
+3.040 -1.093 0.000
+2.910 -0.996 0.000
 2.879 -0.958 0.000
-2.883 -0.941 0.000
-2.888 -0.925 0.000
-2.893 -0.909 0.000
-2.904 -0.896 0.000
-2.940 -0.900 0.000
-3.097 -0.985 0.000
-3.225 -1.052 0.000
-3.272 -1.067 0.000
-3.303 -1.072 0.000
-3.347 -1.085 0.000
-3.404 -1.109 0.000
-3.431 -1.111 0.000
-3.398 -1.071 0.000
-2.967 -0.697 0.000
-2.951 -0.665 0.000
+2.884 -0.942 0.000
+2.889 -0.926 0.000
+2.895 -0.910 0.000
+2.907 -0.898 0.000
+2.943 -0.903 0.000
+3.115 -0.997 0.000
+3.229 -1.055 0.000
+3.276 -1.070 0.000
+3.306 -1.073 0.000
+3.353 -1.089 0.000
+3.409 -1.112 0.000
+3.430 -1.111 0.000
+3.403 -1.075 0.000
+2.973 -0.702 0.000
+2.950 -0.663 0.000
 2.880 -0.588 0.000
-2.842 -0.537 0.000
-2.831 -0.506 0.000
-2.805 -0.241 0.000
-2.815 -0.224 0.000
-2.815 -0.198 0.000
-2.816 -0.172 0.000
-2.814 -0.144 0.000
-2.811 -0.114 0.000
-2.810 -0.087 0.000
-2.812 -0.061 0.000
-2.819 -0.040 0.000
+2.840 -0.535 0.000
+2.834 -0.507 0.000
+2.811 -0.246 0.000
+2.814 -0.223 0.000
+2.814 -0.197 0.000
+2.813 -0.170 0.000
+2.812 -0.142 0.000
+2.814 -0.118 0.000
+2.810 -0.086 0.000
+2.814 -0.063 0.000
+2.820 -0.041 0.000
 4.272 -1.407 0.000
-4.284 -1.407 0.000
-4.291 -1.402 0.000
+4.282 -1.405 0.000
+4.290 -1.402 0.000
 4.304 -1.404 0.000
 4.318 -1.406 0.000
-4.328 -1.406 0.000
-4.342 -1.409 0.000
-4.354 -1.410 0.000
-4.362 -1.407 0.000
-4.373 -1.408 0.000
-4.383 -1.407 0.000
+4.328 -1.405 0.000
+4.343 -1.410 0.000
+4.352 -1.408 0.000
+4.364 -1.410 0.000
+4.374 -1.409 0.000
+4.384 -1.408 0.000
 4.393 -1.406 0.000
 4.402 -1.405 0.000
 4.410 -1.402 0.000
-4.421 -1.404 0.000
+4.419 -1.401 0.000
 4.430 -1.402 0.000
-4.435 -1.396 0.000
-4.446 -1.397 0.000
-4.454 -1.395 0.000
+4.436 -1.397 0.000
+4.445 -1.396 0.000
+4.455 -1.396 0.000
 4.460 -1.390 0.000
-4.466 -1.386 0.000
+4.466 -1.385 0.000
 4.469 -1.376 0.000
-4.473 -1.369 0.000
-4.477 -1.362 0.000
-4.480 -1.353 0.000
-4.480 -1.340 0.000
+4.472 -1.368 0.000
+4.476 -1.361 0.000
+4.481 -1.354 0.000
+4.481 -1.341 0.000
 4.484 -1.331 0.000
 4.483 -1.316 0.000
-4.481 -1.300 0.000
-4.478 -1.282 0.000
-4.474 -1.261 0.000
-4.470 -1.241 0.000
-4.466 -1.220 0.000
-4.468 -1.208 0.000
-4.471 -1.196 0.000
-4.468 -1.176 0.000
+4.482 -1.301 0.000
+4.479 -1.283 0.000
+4.473 -1.261 0.000
+4.468 -1.239 0.000
+4.467 -1.221 0.000
+4.469 -1.209 0.000
+4.470 -1.195 0.000
+4.467 -1.175 0.000
 4.457 -1.143 0.000
-4.451 -1.117 0.000
-4.452 -1.100 0.000
+4.452 -1.118 0.000
+4.452 -1.101 0.000
 4.452 -1.085 0.000
-4.458 -1.076 0.000
+4.457 -1.075 0.000
 4.464 -1.068 0.000
-4.466 -1.054 0.000
-4.462 -1.029 0.000
-4.465 -1.014 0.000
+4.467 -1.055 0.000
+4.463 -1.030 0.000
+4.464 -1.012 0.000
 4.467 -0.999 0.000
 4.470 -0.984 0.000
 4.472 -0.969 0.000
-4.474 -0.951 0.000
-4.477 -0.937 0.000
-4.312 1.396 0.000
-4.336 1.395 0.000
-4.360 1.390 0.000
-4.384 1.390 0.000
-4.406 1.395 0.000
-4.429 1.396 0.000
-4.451 1.402 0.000
-4.472 1.415 0.000
-4.483 1.474 0.000
-4.482 1.599 0.000
-4.483 1.724 0.000
-4.473 1.911 0.000
+4.473 -0.950 0.000
+4.478 -0.939 0.000
+4.312 1.397 0.000
+4.336 1.392 0.000
+4.360 1.391 0.000
+4.384 1.391 0.000
+4.406 1.397 0.000
+4.430 1.395 0.000
+4.452 1.400 0.000
+4.472 1.416 0.000
+4.483 1.475 0.000
+4.482 1.600 0.000
+4.483 1.721 0.000
+4.472 1.915 0.000
 6.042 1.832 0.000
-6.056 1.773 0.000
-6.060 1.662 0.000
-6.064 1.556 0.000
-6.072 1.479 0.000
+6.055 1.766 0.000
+6.059 1.656 0.000
+6.065 1.560 0.000
+6.072 1.477 0.000
 6.093 1.464 0.000
-6.115 1.455 0.000
-6.139 1.457 0.000
-6.162 1.455 0.000
+6.115 1.457 0.000
+6.138 1.456 0.000
+6.162 1.454 0.000
 6.187 1.458 0.000
 6.211 1.459 0.000
 6.235 1.461 0.000
-6.276 1.527 0.000
-7.632 2.092 0.000
-7.603 1.974 0.000
-7.616 1.936 0.000
-6.195 -0.757 0.000
-6.186 -0.797 0.000
+6.278 1.534 0.000
+7.631 2.091 0.000
+7.603 1.973 0.000
+7.615 1.934 0.000
+6.191 -0.765 0.000
+6.187 -0.796 0.000
 6.186 -0.820 0.000
-6.187 -0.839 0.000
-6.190 -0.854 0.000
+6.188 -0.838 0.000
+6.191 -0.854 0.000
 6.193 -0.870 0.000
 6.195 -0.888 0.000
-6.200 -0.900 0.000
-6.204 -0.914 0.000
-6.207 -0.929 0.000
+6.199 -0.901 0.000
+6.204 -0.913 0.000
+6.206 -0.929 0.000
 6.205 -0.951 0.000
-6.204 -0.971 0.000
-6.206 -0.986 0.000
-6.207 -1.003 0.000
-6.209 -1.017 0.000
-6.211 -1.033 0.000
+6.205 -0.969 0.000
+6.207 -0.984 0.000
+6.206 -1.004 0.000
+6.209 -1.018 0.000
+6.210 -1.033 0.000
 6.209 -1.052 0.000
 6.207 -1.073 0.000
-6.202 -1.097 0.000
+6.203 -1.095 0.000
 6.200 -1.115 0.000
 6.200 -1.131 0.000
-6.201 -1.146 0.000
+6.200 -1.147 0.000
 6.204 -1.158 0.000
-6.205 -1.172 0.000
+6.206 -1.171 0.000
 6.208 -1.183 0.000
 6.211 -1.194 0.000
-6.217 -1.202 0.000
-6.221 -1.210 0.000
-6.228 -1.217 0.000
-6.236 -1.221 0.000
+6.217 -1.201 0.000
+6.222 -1.209 0.000
+6.228 -1.216 0.000
+6.235 -1.221 0.000
 6.244 -1.225 0.000
-6.254 -1.227 0.000
-6.264 -1.228 0.000
+6.253 -1.228 0.000
+6.263 -1.230 0.000
 6.278 -1.226 0.000
-6.288 -1.228 0.000
-6.300 -1.229 0.000
-6.314 -1.226 0.000
-6.325 -1.228 0.000
-6.339 -1.226 0.000
-6.353 -1.225 0.000
-6.364 -1.227 0.000
-6.382 -1.221 0.000
-6.393 -1.223 0.000
-6.406 -1.223 0.000
+6.289 -1.228 0.000
+6.299 -1.230 0.000
+6.314 -1.227 0.000
+6.325 -1.227 0.000
+6.338 -1.227 0.000
+6.352 -1.226 0.000
+6.363 -1.228 0.000
+6.380 -1.223 0.000
+6.394 -1.222 0.000
+6.405 -1.224 0.000
 7.763 -0.634 0.000
-7.762 -0.656 0.000
-7.760 -0.680 0.000
+7.762 -0.657 0.000
+7.759 -0.681 0.000
 7.759 -0.702 0.000
-7.761 -0.722 0.000
+7.760 -0.723 0.000
 7.766 -0.740 0.000
-7.771 -0.757 0.000
-7.776 -0.775 0.000
-7.782 -0.792 0.000
-7.786 -0.810 0.000
-7.788 -0.830 0.000
-7.787 -0.851 0.000
+7.770 -0.758 0.000
+7.774 -0.776 0.000
+7.781 -0.793 0.000
+7.785 -0.811 0.000
+7.787 -0.830 0.000
+7.788 -0.850 0.000
 7.789 -0.870 0.000
-7.792 -0.888 0.000
-7.793 -0.907 0.000
+7.793 -0.888 0.000
+7.792 -0.908 0.000
 7.792 -0.928 0.000
-7.789 -0.950 0.000
-7.787 -0.971 0.000
+7.791 -0.949 0.000
+7.788 -0.970 0.000
 7.784 -0.992 0.000
 7.782 -1.012 0.000
-7.782 -1.031 0.000
-7.782 -1.050 0.000
-7.785 -1.068 0.000
-7.789 -1.084 0.000
-7.794 -1.100 0.000
-7.799 -1.116 0.000
+7.783 -1.031 0.000
+7.783 -1.050 0.000
+7.786 -1.067 0.000
+7.787 -1.085 0.000
+7.792 -1.101 0.000
+7.800 -1.116 0.000
 7.817 -1.126 0.000
-7.838 -1.134 0.000
-7.868 -1.138 0.000
-7.909 -1.136 0.000
+7.834 -1.136 0.000
+7.869 -1.137 0.000
+7.911 -1.136 0.000
 7.943 -1.139 0.000

cloud_scan4.txt

@@ -1,84 +1,85 @@
-12.088 -2.462 0.000
-7.965 -0.899 0.000
-7.959 -0.788 0.000
+12.086 -2.458 0.000
+7.965 -0.900 0.000
+7.960 -0.785 0.000
 7.958 -0.663 0.000
-7.944 -0.607 0.000
+7.944 -0.605 0.000
 7.923 -0.588 0.000
-7.901 -0.579 0.000
-7.876 -0.577 0.000
-7.852 -0.578 0.000
+7.901 -0.580 0.000
+7.876 -0.579 0.000
+7.852 -0.577 0.000
 7.827 -0.578 0.000
-7.802 -0.580 0.000
-7.777 -0.582 0.000
-7.753 -0.578 0.000
-7.864 1.654 0.000
-7.862 1.675 0.000
-7.863 1.703 0.000
-7.862 1.725 0.000
+7.802 -0.581 0.000
+7.776 -0.584 0.000
+7.752 -0.580 0.000
+7.864 1.653 0.000
+7.861 1.674 0.000
+7.864 1.705 0.000
+7.861 1.723 0.000
 7.858 1.742 0.000
 7.854 1.759 0.000
-7.848 1.772 0.000
-7.845 1.788 0.000
-7.839 1.802 0.000
-7.840 1.824 0.000
-7.838 1.845 0.000
-7.838 1.865 0.000
-7.837 1.886 0.000
+7.850 1.774 0.000
+7.844 1.788 0.000
+7.839 1.801 0.000
+7.839 1.824 0.000
+7.839 1.845 0.000
+7.837 1.864 0.000
+7.837 1.885 0.000
 7.838 1.906 0.000
-7.837 1.925 0.000
-7.838 1.947 0.000
-7.841 1.971 0.000
-7.847 2.000 0.000
-7.848 2.022 0.000
-7.849 2.040 0.000
+7.836 1.925 0.000
+7.837 1.946 0.000
+7.840 1.970 0.000
+7.846 1.999 0.000
+7.849 2.022 0.000
+7.848 2.039 0.000
 7.846 2.054 0.000
-7.843 2.068 0.000
+7.844 2.069 0.000
 7.839 2.078 0.000
-7.835 2.090 0.000
-7.830 2.099 0.000
-7.823 2.106 0.000
+7.834 2.088 0.000
+7.829 2.098 0.000
+7.822 2.105 0.000
 7.817 2.113 0.000
-7.810 2.120 0.000
+7.809 2.120 0.000
 7.799 2.122 0.000
-7.789 2.123 0.000
-7.780 2.127 0.000
+7.789 2.124 0.000
+7.779 2.127 0.000
 7.766 2.124 0.000
-7.756 2.127 0.000
-7.741 2.123 0.000
-7.729 2.124 0.000
-7.716 2.123 0.000
+7.754 2.125 0.000
+7.741 2.124 0.000
+7.729 2.123 0.000
+7.715 2.122 0.000
 7.704 2.124 0.000
 7.692 2.124 0.000
 7.679 2.124 0.000
-7.665 2.123 0.000
-7.652 2.122 0.000
-7.638 2.122 0.000
+7.664 2.122 0.000
+7.651 2.121 0.000
+7.637 2.120 0.000
+7.624 2.121 0.000
 6.294 1.572 0.000
-6.293 1.594 0.000
-6.294 1.617 0.000
-6.293 1.639 0.000
+6.292 1.593 0.000
+6.293 1.617 0.000
+6.294 1.639 0.000
 6.293 1.661 0.000
-6.290 1.680 0.000
-6.286 1.699 0.000
-6.285 1.720 0.000
-6.282 1.739 0.000
-6.277 1.757 0.000
-6.274 1.776 0.000
-6.271 1.794 0.000
-6.270 1.815 0.000
-6.272 1.836 0.000
-6.274 1.858 0.000
-6.275 1.879 0.000
-6.276 1.900 0.000
-6.277 1.921 0.000
-6.277 1.940 0.000
-6.273 1.958 0.000
-6.272 1.977 0.000
-6.268 1.994 0.000
+6.289 1.680 0.000
+6.287 1.700 0.000
+6.283 1.718 0.000
+6.281 1.738 0.000
+6.275 1.755 0.000
+6.274 1.775 0.000
+6.271 1.795 0.000
+6.271 1.815 0.000
+6.271 1.836 0.000
+6.275 1.859 0.000
+6.273 1.878 0.000
+6.277 1.901 0.000
+6.275 1.919 0.000
+6.275 1.939 0.000
+6.272 1.957 0.000
+6.271 1.977 0.000
+6.266 1.992 0.000
 6.261 2.009 0.000
-6.252 2.023 0.000
-6.235 2.033 0.000
-6.211 2.039 0.000
-6.179 2.040 0.000
-6.138 2.038 0.000
+6.251 2.023 0.000
+6.236 2.034 0.000
+6.214 2.041 0.000
+6.181 2.042 0.000
+6.139 2.038 0.000
 6.099 2.037 0.000

cloud_scan5.txt

@@ -1,99 +1,100 @@
-13.081 1.568 0.000
-13.081 1.456 0.000
+13.080 1.562 0.000
+13.080 1.452 0.000
 13.089 1.387 0.000
-13.085 1.272 0.000
+13.084 1.269 0.000
 13.099 1.234 0.000
-13.119 1.224 0.000
-13.140 1.217 0.000
-13.163 1.218 0.000
+13.118 1.221 0.000
+13.140 1.216 0.000
+13.163 1.219 0.000
 13.186 1.218 0.000
-13.209 1.219 0.000
+13.209 1.218 0.000
 13.233 1.221 0.000
-13.327 1.384 0.000
-13.067 -0.853 0.000
+13.325 1.377 0.000
+13.382 1.484 0.000
+13.064 -0.859 0.000
 13.054 -0.901 0.000
-13.052 -0.924 0.000
-13.056 -0.938 0.000
-13.060 -0.952 0.000
-13.063 -0.968 0.000
+13.054 -0.921 0.000
+13.056 -0.939 0.000
+13.059 -0.954 0.000
+13.062 -0.969 0.000
 13.064 -0.985 0.000
-13.066 -1.002 0.000
-13.071 -1.013 0.000
-13.074 -1.027 0.000
-13.075 -1.043 0.000
-13.063 -1.080 0.000
-13.054 -1.113 0.000
-13.052 -1.133 0.000
-13.052 -1.150 0.000
-13.054 -1.164 0.000
+13.065 -1.003 0.000
+13.070 -1.014 0.000
+13.073 -1.028 0.000
+13.075 -1.044 0.000
+13.063 -1.081 0.000
+13.053 -1.115 0.000
+13.051 -1.134 0.000
+13.051 -1.151 0.000
+13.053 -1.165 0.000
 13.060 -1.171 0.000
-13.069 -1.173 0.000
-13.071 -1.187 0.000
-13.061 -1.216 0.000
-13.058 -1.237 0.000
-13.059 -1.250 0.000
-13.056 -1.269 0.000
-13.051 -1.290 0.000
+13.068 -1.175 0.000
+13.069 -1.190 0.000
+13.060 -1.219 0.000
+13.058 -1.236 0.000
+13.058 -1.252 0.000
+13.056 -1.270 0.000
+13.050 -1.292 0.000
 13.050 -1.306 0.000
 13.051 -1.319 0.000
-13.055 -1.328 0.000
+13.054 -1.328 0.000
 13.058 -1.337 0.000
-13.062 -1.345 0.000
-13.068 -1.351 0.000
-13.072 -1.360 0.000
-13.080 -1.362 0.000
-13.088 -1.366 0.000
-13.095 -1.370 0.000
-13.105 -1.370 0.000
+13.061 -1.346 0.000
+13.067 -1.352 0.000
+13.070 -1.361 0.000
+13.079 -1.364 0.000
+13.086 -1.367 0.000
+13.094 -1.371 0.000
+13.105 -1.371 0.000
 13.114 -1.372 0.000
-13.124 -1.373 0.000
-13.134 -1.375 0.000
-13.144 -1.376 0.000
-13.156 -1.375 0.000
-13.166 -1.377 0.000
+13.124 -1.374 0.000
+13.133 -1.376 0.000
+13.143 -1.377 0.000
+13.154 -1.378 0.000
+13.164 -1.379 0.000
 13.176 -1.378 0.000
 13.186 -1.380 0.000
-13.199 -1.378 0.000
-13.210 -1.379 0.000
-13.218 -1.383 0.000
-14.572 -0.718 0.000
-14.594 -0.724 0.000
-14.619 -0.728 0.000
-14.643 -0.732 0.000
+13.198 -1.379 0.000
+13.208 -1.382 0.000
+13.217 -1.384 0.000
+14.570 -0.720 0.000
+14.592 -0.725 0.000
+14.618 -0.728 0.000
+14.643 -0.731 0.000
 14.626 -0.764 0.000
-14.538 -0.846 0.000
-14.530 -0.872 0.000
-14.527 -0.894 0.000
-14.524 -0.916 0.000
-14.529 -0.931 0.000
-14.535 -0.947 0.000
-14.539 -0.964 0.000
-14.551 -0.975 0.000
+14.533 -0.850 0.000
+14.529 -0.872 0.000
+14.524 -0.896 0.000
+14.523 -0.916 0.000
+14.528 -0.932 0.000
+14.533 -0.948 0.000
+14.539 -0.963 0.000
+14.550 -0.976 0.000
 14.566 -0.985 0.000
-14.586 -0.991 0.000
-14.598 -1.003 0.000
-14.591 -1.027 0.000
+14.586 -0.992 0.000
+14.600 -1.002 0.000
+14.592 -1.026 0.000
 14.552 -1.069 0.000
-14.533 -1.098 0.000
-14.517 -1.125 0.000
-14.520 -1.142 0.000
-14.526 -1.156 0.000
-14.533 -1.170 0.000
+14.531 -1.099 0.000
+14.516 -1.126 0.000
+14.518 -1.143 0.000
+14.525 -1.157 0.000
+14.532 -1.171 0.000
 14.560 -1.173 0.000
-14.599 -1.170 0.000
-14.609 -1.182 0.000
-14.617 -1.196 0.000
-14.665 -1.189 0.000
-14.590 -1.245 0.000
-14.538 -1.289 0.000
-14.529 -1.311 0.000
-14.522 -1.331 0.000
-14.519 -1.349 0.000
+14.598 -1.170 0.000
+14.608 -1.183 0.000
+14.618 -1.196 0.000
+14.664 -1.190 0.000
+14.593 -1.244 0.000
+14.539 -1.289 0.000
+14.527 -1.311 0.000
+14.521 -1.331 0.000
+14.518 -1.349 0.000
 14.520 -1.365 0.000
 14.524 -1.379 0.000
-14.530 -1.393 0.000
-14.545 -1.403 0.000
+14.529 -1.394 0.000
+14.542 -1.404 0.000
 14.567 -1.410 0.000
-14.599 -1.412 0.000
+14.598 -1.413 0.000
 14.635 -1.413 0.000
-14.707 -1.399 0.000
+14.716 -1.396 0.000

src/findwheel/src/FenceController.cpp

@@ -240,26 +240,33 @@ void FenceController::wheelMsgHandlingThread(FenceController* fc){
                             fc->m_cloud_mutex.unlock();
                             // std::cout << "--------callback get cloud, size: "<<total_cloud->size()<<"------" << std::endl;
                             LOG(INFO)<< "--------callback get cloud, size: "<<total_cloud->size();
+
+                            char cloud_txt_filename[255];
+                            memset(cloud_txt_filename, 0, 255);
+                            char whole_filename[255];
+                            memset(whole_filename, 0, 255);
                             if (total_cloud->size() > 0 &&
                                 fc->p_region_workers[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width))
                             {
                                 result.set_correctness(true);
-                                // save cloud txt
-                                if (fc->m_global_param.has_data_path()&& fc->m_global_param.data_path() != "")
-                                {
-                                    PathCreator path_creator;
-                                    path_creator.CreateDatePath(fc->m_global_param.data_path(), false);
-                                    char cloud_txt_filename[255];
-                                    memset(cloud_txt_filename, 0, 255);
-                                    sprintf(cloud_txt_filename, "%s/%s.txt", path_creator.GetCurPath().c_str(), result.time().c_str());
-                                    fc->save_cloud_txt(cloud_txt_filename, total_cloud);
-                                    // std::cout << " save cloud   " << cloud_txt_filename << std::endl;
-                                    LOG(INFO)<<" save cloud " << cloud_txt_filename;
-                                }
+                                sprintf(cloud_txt_filename, "/%s.txt", result.time().c_str());
                             }
                             else
                             {
                                 result.set_correctness(false);
+                                sprintf(cloud_txt_filename, "/%s%s.txt", result.time().c_str(), "_failed");
+                            }
+
+                            // save cloud txt
+                            if (fc->m_global_param.has_data_path() && fc->m_global_param.data_path() != "")
+                            {
+                                PathCreator path_creator;
+                                path_creator.CreateDatePath(fc->m_global_param.data_path(), false);
+                                sprintf(whole_filename, "%s%s", path_creator.GetCurPath().c_str(), cloud_txt_filename);
+                                
+                                fc->save_cloud_txt(whole_filename, total_cloud);
+                                // std::cout << " save cloud   " << whole_filename << std::endl;
+                                LOG(INFO) << " save cloud " << whole_filename;
                             }
                             //  std::cout << "--------callback get result------" << std::endl;
                             LOG(INFO)<<"--------callback get result------";

src/findwheel/src/region_detect.cpp

@@ -271,9 +271,9 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
 {
     bool result = false;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    preprocess(cloud_in, cloud_filtered);
+    result = preprocess(cloud_in, cloud_filtered);
 
-    if (cloud_filtered->size() <= 0)
+    if (cloud_filtered->size() <= 0 || !result)
         return false;
     std::vector<cv::Point2f> corner_points;
     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
@@ -285,15 +285,17 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, doub
 {
     bool result = false;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
-    preprocess(cloud_in, cloud_filtered);
+    result = preprocess(cloud_in, cloud_filtered);
     // std::cout<<"--------detector after preprocess------"<<std::endl;
 
-    if (cloud_filtered->size() <= 0)
+    if (cloud_filtered->size() <= 0 || !result)
         return false;
     std::vector<cv::Point2f> corner_points;
     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
-    result = clustering(cloud_filtered, corner_points, seg_clouds, true);
+    result &= clustering(cloud_filtered, corner_points, seg_clouds, true);
     // add four corner points into input cloud
+    cloud_in->clear();
+    cloud_in->operator+=(*cloud_filtered);
     for (size_t i = 0; i < corner_points.size(); i++)
     {
         cloud_in->points.push_back(pcl::PointXYZ(corner_points[i].x, corner_points[i].y, 0.0));

src/findwheel/src/region_worker.cpp

@@ -36,7 +36,7 @@ int Region_worker::get_id(){
     // m_mutex.lock();
     std::lock_guard<std::mutex> lck(m_mutex);
     if(m_detector){
-        id = m_detector->m_region_id;
+        id = m_detector->m_region_id + 1;
     }else{
         id = -1;
     }