From 7f7983c1ae0d7299fff6c4abe7455db682243fa0 Mon Sep 17 00:00:00 2001 From: Eason Date: Thu, 12 Dec 2024 14:16:17 +0800 Subject: [PATCH 1/2] fast_planner merge --- .../exploration_manager/config/rviz.rviz | 16 ++-- .../src/exploration_manager.cpp | 2 +- .../resource/coverage_path.tsp | 94 ++----------------- .../resource/coverage_path.txt | 94 ++----------------- .../fast_planner/src/planner_manager.cpp | 91 ++++++++++++------ .../include/polynomial/polynomial_traj.h | 5 + .../src/polynomial/polynomial_traj.cpp | 36 +++++++ 7 files changed, 127 insertions(+), 211 deletions(-) diff --git a/falcon_planner/exploration_manager/config/rviz.rviz b/falcon_planner/exploration_manager/config/rviz.rviz index dad40a5..6c6962a 100644 --- a/falcon_planner/exploration_manager/config/rviz.rviz +++ b/falcon_planner/exploration_manager/config/rviz.rviz @@ -4,7 +4,6 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Trajectory1 - /Trajectory1/bspline_traj1/Namespaces1 - /Trajectory1/position_cmd1 - /Trajectory1/position_cmd1/Namespaces1 @@ -165,7 +164,6 @@ Visualization Manager: Marker Topic: /planning/position_cmd_vis Name: position_cmd Namespaces: - current_cmd: true current_fov: true Queue Size: 100 Value: true @@ -907,7 +905,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 22.538219451904297 + Distance: 38.2598991394043 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -915,17 +913,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -0.7072279453277588 - Y: -8.028024673461914 - Z: -4.76837158203125e-06 - Focal Shape Fixed Size: false + X: 3 + Y: -7.5 + Z: -5.5 + Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.41039881110191345 + Pitch: 0.4000000059604645 Target Frame: - Yaw: 3.030402660369873 + Yaw: 3.5999999046325684 Saved: - Class: rviz/ThirdPersonFollower Distance: 24 diff --git a/falcon_planner/exploration_manager/src/exploration_manager.cpp b/falcon_planner/exploration_manager/src/exploration_manager.cpp index 811f2c2..24ebbdc 100644 --- a/falcon_planner/exploration_manager/src/exploration_manager.cpp +++ b/falcon_planner/exploration_manager/src/exploration_manager.cpp @@ -575,7 +575,7 @@ int ExplorationManager::planExploreMotionHGrid(const Vector3d &pos, const Vector double sop_time = (ros::Time::now() - t1).toSec(); ROS_INFO("[ExplorationManager] SOP time: %.2f ms", sop_time * 1000.0); - CHECK_LE(sop_time, 0.5) << "SOP solver internal error detected, solver blocked with unknown " + CHECK_LE(sop_time, 1.0) << "SOP solver internal error detected, solver blocked with unknown " "error. Please restart the planner"; ee_->sop_times_.push_back(make_pair(sop_cost_matrix_time, sop_time)); diff --git a/falcon_planner/exploration_utils/resource/coverage_path.tsp b/falcon_planner/exploration_utils/resource/coverage_path.tsp index af7163a..702287d 100644 --- a/falcon_planner/exploration_utils/resource/coverage_path.tsp +++ b/falcon_planner/exploration_utils/resource/coverage_path.tsp @@ -1,93 +1,13 @@ NAME : coverage_path TYPE : ATSP -DIMENSION : 86 +DIMENSION : 6 EDGE_WEIGHT_TYPE : EXPLICIT EDGE_WEIGHT_FORMAT : FULL_MATRIX EDGE_WEIGHT_SECTION -0 201829 201493 201487 202117 201835 201830 202783 202575 202571 203332 203160 203157 204300 914 201456 696 200771 507 201001 1013 201863 201328 201343 50680 202595 202242 202175 101804 203294 101713 202906 102161 204293 102282 204110 102186 205038 204870 204848 201368 157 200573 338 200570 201768 201194 795 201199 202527 202165 888 202282 203413 101687 203084 101657 203111 102034 204349 101986 204155 101956 204165 204999 204825 204837 201596 200918 201039 201971 201476 840 201534 202673 202332 202339 203522 203271 203297 204435 204238 204258 205093 204922 204940 -0 0 379 720 379 537 814 879 958 1137 1219 1277 1416 2357 1159 1351 1081 1331 1849 1327 1307 1404 1469 1578 1541 1774 1838 1859 2135 2500 2682 2128 2558 2283 3062 3048 3602 2793 3175 3986 2358 2242 1261 2033 1706 2399 2507 2266 2880 2708 2795 2870 2641 2165 3008 2627 3321 3178 2848 2665 3755 3125 4350 3587 3005 3505 3925 3372 1575 1998 3495 3547 2644 3779 3732 3776 2970 2540 3050 3509 3040 3550 4009 3390 3900 4359 -0 379 0 340 537 379 509 958 879 943 1277 1219 1266 1977 1383 1556 701 1209 1588 984 1381 1544 1351 1323 1921 1888 1766 1681 2515 2433 1905 2084 2938 2663 3407 2491 2788 3173 3555 3684 2494 2191 881 2501 1326 2526 2427 1914 1623 2805 2735 2365 2135 2545 2390 2121 2703 2673 3228 3045 2591 2661 3207 3123 3385 3004 3424 3606 1195 1618 3588 3489 2180 1946 2420 3727 2443 2920 2522 2981 3420 3022 3481 3770 3372 3831 -0 720 340 0 814 509 379 1137 943 879 1416 1266 1219 2317 1696 1835 1041 1357 1456 732 1721 1804 1448 1244 2261 2080 1824 1634 2855 2445 2245 2138 3278 3003 3747 2831 3128 3513 3895 3750 2712 2426 1221 1993 1666 2734 2491 1574 1283 2973 2784 2311 2081 2885 2730 2461 2997 2606 3568 3385 2931 3001 3415 3148 3725 3344 3490 1871 1535 1958 3753 3536 1840 1606 2760 3766 2308 3260 2862 2931 3760 3362 3472 4110 3712 3854 -0 379 537 814 0 379 720 500 628 876 840 921 1106 1977 1388 1556 1461 1497 1991 1352 1266 1217 1351 1489 1366 1451 1544 1616 1755 2004 2225 1790 2178 1903 2682 2668 3222 2413 2795 3606 2494 2461 2752 2413 2086 2311 2427 1886 2816 2458 2561 2490 2261 2801 2628 2842 2941 2798 2468 2285 3375 2745 3970 3207 2625 3125 3545 3472 3524 2378 3433 3489 2264 3729 3537 3588 3870 2160 2670 3129 2660 3170 3629 3010 3520 3979 -0 537 379 509 379 0 340 628 500 604 921 840 906 1597 1435 1712 1453 1410 1684 1100 1001 1440 1199 1188 1536 1612 1440 1376 2135 2111 2179 1731 2558 2283 3027 2111 2408 2793 3175 3304 2612 2421 1261 2033 1706 2451 2339 1534 2630 2578 2486 1985 1755 2891 2010 2797 2323 2293 2848 2665 2211 2281 2827 2743 3005 2624 3044 3564 3466 3869 3532 3427 1800 3677 3628 3532 3729 2540 2142 2601 3040 2642 3101 3390 2992 3451 -0 814 509 379 720 340 0 876 604 500 1106 906 840 1937 1802 1952 1421 1518 1640 998 1341 1730 1324 1082 1881 1856 1524 1305 2225 2295 2236 1804 2898 2623 3367 2451 2748 3133 3515 3370 2810 2501 2767 1737 1450 2672 2410 1194 2663 2777 2546 1931 1701 2505 2350 2853 2617 2990 3188 3005 2551 2621 3035 2768 3345 2964 3110 3726 3512 1766 3703 3477 1460 1226 2380 3577 1928 2880 2482 2551 3380 2982 3092 3730 3332 3474 -0 879 958 1137 500 628 876 0 379 720 340 509 796 1477 1555 1979 1961 1894 2564 1703 1121 1509 1544 1637 1132 1217 1351 1490 1211 1608 1814 1425 2169 2266 2138 2168 2678 1913 2650 3062 2833 2855 2918 2913 2586 2458 2561 2386 2924 2311 2427 1990 1761 2458 2477 2527 2604 2298 2355 2833 2610 2873 2881 2707 2125 2625 3045 2031 2455 2878 3537 3588 2764 3815 3433 3489 3697 3537 3588 3783 2160 2670 3129 2510 3020 3479 -0 958 879 943 628 500 604 379 0 340 509 340 480 1097 1935 2081 1652 1840 2133 1526 1620 1612 1440 1405 1353 1440 1199 1190 1663 1770 1745 1326 2297 2378 2527 1943 1908 2293 2675 2804 2923 2826 2873 2531 2390 2578 2486 1988 2920 2451 2339 1485 1255 2578 2354 2466 1823 1793 2379 2923 2534 2828 2327 3126 2505 2124 2544 2411 2143 2602 3628 3532 2237 2062 3532 3427 3545 3628 3532 2101 2540 2142 2601 2890 2492 2951 -0 1137 943 879 876 604 500 720 340 0 796 480 340 1437 2275 2258 1992 1907 2146 1469 1841 1856 1524 1337 1614 1730 1324 1227 1896 1565 1904 1448 2355 2516 2867 1951 2248 2633 3015 2870 2411 2884 2929 2809 2336 2777 2546 1934 1789 2672 2410 2627 1201 2777 1850 2542 2117 1726 2612 2505 2051 2121 2535 2268 2845 2464 2610 2751 2483 2470 2380 3577 2200 1930 3703 3477 1428 2380 3577 2051 2880 2482 2592 3230 2832 2974 -0 1219 1277 1416 840 921 1106 340 509 796 0 379 720 1137 1895 2295 2301 2201 2904 2192 1461 1738 1804 1859 1232 1338 1448 1578 1375 1461 1687 1324 1928 2050 1851 1892 2392 2317 2019 2776 2031 2478 2481 3253 2926 2679 2767 2726 3101 2382 2491 3014 3097 2328 2283 2516 2460 2855 2245 1367 2488 1827 2800 3034 1707 2207 2627 2371 2795 3218 3708 3753 3104 2930 3483 3536 3798 3444 1752 3706 1742 2252 2711 2092 2602 3061 -0 1277 1219 1266 921 840 906 509 340 480 379 0 340 1651 2275 2376 1992 2160 2596 1844 1841 1856 1730 1681 1464 1524 1324 1323 1526 1657 1574 1201 1728 1507 2073 1537 2613 2017 2399 2846 2411 2334 2043 2871 2730 2777 2706 2328 2183 2512 2410 1825 1595 2464 2216 2350 2311 2699 2305 1747 2766 2705 3362 2003 2087 2898 3065 2751 2483 2942 2380 3703 2577 4017 3577 3477 3658 1622 3438 1626 2122 1906 2365 2472 2246 2705 -0 1416 1266 1219 1106 906 840 796 480 340 720 340 0 1097 2615 2411 2332 2212 2506 1801 2181 2053 1790 1634 1799 1790 1425 1298 1644 1930 1746 1351 2068 1847 2413 1497 2953 2357 2739 3186 2751 2674 2383 2817 2676 2949 2755 2274 2985 2722 2476 1771 1541 2684 1350 2566 1663 1692 2357 2087 3106 1881 3702 2343 2427 2224 2644 3091 2823 2810 2720 3901 2540 2270 3743 3524 1768 1962 1507 1966 2462 2246 2705 2812 2586 3045 -0 2357 1977 2317 1977 1597 1937 1477 1097 1437 1137 1651 1097 0 3934 3152 2864 2573 3937 2913 3376 2651 2111 2376 2017 2165 1954 2003 2242 2017 1574 1664 1819 1165 1315 1199 1856 1675 1693 2088 3518 3000 2709 4754 3447 3018 2285 4211 2899 2518 2877 3708 2312 2683 2252 2541 2391 2813 2064 1462 2098 2342 2485 2632 1802 2551 1546 3894 3149 3650 3393 2648 4477 3110 2893 2148 2609 2397 1648 2107 3677 3428 1607 2175 3632 1948 -0 1159 1383 1696 1388 1435 1802 1555 1935 2275 1895 2275 2615 3934 0 439 419 786 684 1145 232 691 991 1163 501 1175 1360 1438 845 1757 1119 1638 1057 2191 1309 2956 1579 2701 3083 3543 1407 746 1012 827 1456 1584 1867 1116 2289 2002 2203 1345 2548 2073 1282 2534 1438 3086 1202 2573 1655 3033 1953 3494 2913 3413 3833 2415 2676 1807 2620 2812 1249 3136 2949 3098 2878 2448 2958 3417 2948 3458 3917 3298 3808 4267 -0 1351 1556 1835 1556 1712 1952 1979 2081 2258 2295 2376 2411 3152 439 0 832 543 963 941 749 581 788 1106 1250 1080 1198 1412 1895 1500 2442 1497 2319 1987 2822 2752 3362 2497 2879 3747 1020 1069 808 1859 1252 1252 1464 2437 1872 1707 1848 2895 2460 1869 2768 2331 3081 2882 2608 2369 3515 2829 4111 3290 2709 3209 3629 2039 2265 2731 2267 2401 2619 2710 2627 2725 2674 2244 2754 3213 2744 3254 3713 3094 3604 4063 -0 1081 701 1041 1461 1453 1421 1961 1652 1992 2301 1992 2332 2864 419 832 0 473 425 689 634 1053 912 1093 900 1756 1464 1558 1197 2183 939 1595 1408 2742 1690 2464 1342 3252 3634 3233 1697 634 763 476 2089 1915 1828 765 2102 2314 2263 994 2862 2623 1181 2094 1262 2646 1553 3123 1282 2634 1478 3096 3463 2977 3397 2756 2591 2892 3021 2882 898 3099 2498 3171 2416 2998 2495 2954 3498 2995 3454 3848 3345 3804 -0 1331 1209 1357 1497 1410 1518 1894 1840 1907 2201 2160 2212 2573 786 543 473 0 595 769 922 650 461 653 1264 1064 960 1061 2685 1616 1586 1300 3109 2451 3089 2173 2975 2961 3343 3524 1403 1012 1387 1243 916 1482 1357 1822 1654 1861 1793 2280 2194 2332 2071 2229 2816 2355 3398 2832 2272 2343 3247 2805 3172 2685 3105 2422 2278 1208 2472 2359 2088 2520 2782 2699 2725 2707 2204 2663 3207 2704 3163 3557 3054 3513 -0 1849 1588 1456 1991 1684 1640 2564 2133 2146 2904 2596 2506 3937 684 963 425 595 0 1372 752 1202 904 766 1201 2359 1856 2161 1498 2787 1060 2198 1710 3345 1310 3067 1040 3855 2998 2630 1828 570 1366 472 1514 1823 1591 921 1219 2727 2029 974 1615 3227 1223 2697 961 2126 1855 3727 1323 3237 1176 2531 4067 3580 2890 2037 2364 2245 2602 2098 957 1542 3102 2598 1842 3602 3098 2451 4102 3598 2855 4452 3948 3254 -0 1327 984 732 1352 1100 998 1703 1526 1469 2192 1844 1801 2913 1145 941 689 769 1372 0 2029 1395 993 829 1617 1943 1439 996 3163 2370 2064 1837 3587 2929 3567 2651 3453 3439 3821 3484 2305 1829 2044 1981 1394 2269 1977 1369 2177 2539 2391 2045 2698 2810 2549 2281 2731 2340 3876 3310 2750 2821 3149 2882 3650 3163 3224 3134 2941 3114 2185 3071 1635 3132 2685 3271 3217 3185 2682 2665 3685 3182 3206 4035 3532 3588 -0 1307 1381 1721 1266 1001 1341 1121 1620 1841 1461 1841 2181 3376 232 749 634 922 752 2029 0 447 882 1248 351 882 1152 1434 566 1465 982 1368 778 1690 1030 2455 1300 2200 2582 2985 1639 881 1996 1106 1929 1491 1805 1458 2354 1772 2009 1138 2047 2202 1162 2323 1159 2585 923 2071 1376 2531 1674 2993 2411 2911 3331 2609 2791 3294 2604 2793 1500 3127 2793 2954 3257 3143 2456 2915 2446 2956 3415 2796 3306 3765 -0 1404 1544 1804 1217 1440 1730 1509 1612 1856 1738 1856 2053 2651 691 581 1053 650 1202 1395 447 0 500 880 672 500 707 1003 1337 1083 1884 957 1761 1670 2264 2251 2804 1996 2378 3189 1388 1605 1591 2416 2283 1102 1388 1469 1842 1355 1562 2073 1843 1800 1896 1890 2523 2329 1896 1867 2957 2327 3553 2789 2207 2707 3127 2316 2449 2829 2227 2380 2578 2713 2380 2510 2710 2730 2827 2711 2242 2752 3211 2592 3102 3561 -0 1469 1351 1448 1351 1199 1324 1544 1440 1524 1804 1730 1790 2111 991 788 912 461 904 993 882 500 0 382 997 707 500 630 1837 1210 1283 840 2261 1986 2627 1711 2008 2496 2878 2904 1562 1479 1435 1524 1237 1355 1140 2015 1469 1531 1388 1585 1355 1911 1762 1793 1923 2162 1900 2367 1811 1881 2427 2343 2707 2224 2644 2449 2316 2542 2380 2227 2148 2388 2510 2380 2412 2827 2730 2201 2742 2242 2701 3092 2592 3051 -0 1578 1323 1244 1489 1188 1082 1637 1405 1337 1859 1681 1634 2376 1163 1106 1093 653 766 829 1248 880 382 0 1546 1024 664 460 2140 1537 1538 987 2564 2288 2930 2014 2311 2798 3180 2948 1872 1703 1644 1472 1088 1712 1356 2037 1599 1842 1558 1509 1279 2158 2004 1941 2195 2158 2853 2670 2113 2184 2613 2345 3010 2526 2688 2731 2477 2733 2684 2403 2181 2556 2790 2540 2598 2545 2869 3009 3045 2545 2669 3395 2895 3051 -0 1541 1921 2261 1366 1536 1881 1132 1353 1614 1232 1464 1799 2017 501 1250 900 1264 1201 1617 351 672 997 1546 0 273 745 961 405 776 600 803 687 1342 760 1951 1030 1670 1818 2445 2023 1159 2178 1376 2425 1570 1813 1417 2348 1349 1694 886 1543 1570 951 1765 889 2081 809 2023 1008 2146 1404 2577 2339 2407 2827 2883 3005 2912 2610 2768 1509 3186 2471 2651 2942 2610 2768 3054 2942 3063 2911 2292 2802 3261 -0 1774 1888 2080 1451 1612 1856 1217 1440 1730 1338 1524 1790 2165 1175 1080 1756 1064 2359 1943 882 500 707 1024 273 0 500 881 793 587 1088 572 1166 1175 1720 1751 2260 1640 1627 2645 1829 2105 1951 2953 2451 1355 1562 2392 1969 1102 1388 1573 1343 1355 1554 1513 1979 2138 1415 1829 2413 1924 2031 2348 2152 2423 2627 2672 2769 2712 2380 2510 2743 2813 2227 2380 2608 2380 2510 2790 2730 2827 2711 2092 2602 3061 -0 1838 1766 1824 1544 1440 1524 1351 1199 1324 1448 1324 1425 1954 1360 1198 1464 960 1856 1439 1152 707 500 664 745 500 0 385 1293 872 786 342 1717 1453 2127 1211 1508 1996 2378 2404 1940 1979 1852 2131 1990 1531 1388 1588 1662 1355 1140 1085 855 1531 1391 1357 1423 1711 1421 1940 1576 1928 1927 2160 2239 2152 2144 2769 2672 2874 2510 2380 2327 2521 2380 2227 2345 2510 2380 2486 2827 2730 2201 2592 2092 2551 -0 1859 1681 1634 1616 1376 1305 1490 1190 1227 1578 1323 1298 2003 1438 1412 1558 1061 2161 996 1434 1003 630 460 961 881 385 0 1129 1232 1076 591 1510 1694 2432 1516 1814 2301 2683 2488 2158 1900 1969 2095 1954 1823 1531 2043 1760 1713 1357 1616 1739 1862 1542 1576 1735 1743 1786 2217 1616 2164 1883 2271 2512 2490 2519 2982 2780 3041 2774 2520 2268 2730 2684 2403 2403 2807 2561 2630 2547 3022 3058 2897 2397 2591 -0 2135 2515 2855 1755 2135 2225 1211 1663 1896 1375 1526 1644 2242 845 1895 1197 2685 1498 3163 566 1337 1837 2140 405 793 1293 1129 0 570 314 530 297 620 463 674 665 925 1081 1851 2150 1432 2574 1673 3008 2094 2150 1187 2461 1594 1614 936 2292 1111 627 1368 764 1667 455 1326 746 1489 894 2068 1609 1745 2414 2526 3036 3496 2996 2887 1320 2956 2515 2551 2744 2231 2383 2693 2338 2473 2883 2554 2665 2915 -0 2500 2433 2445 2004 2111 2295 1608 1770 1565 1461 1657 1930 2017 1757 1500 2183 1616 2787 2370 1465 1083 1210 1537 776 587 872 1232 570 0 501 707 585 640 1282 1062 1231 920 1061 2207 2051 2510 2136 3702 2653 1538 1689 2393 2037 1083 1318 2227 1770 873 1091 1150 1452 1699 852 1252 1975 1398 1656 1819 1567 1842 1935 2170 2680 3140 2458 2564 2997 2828 2109 2255 2476 1992 2158 2485 2205 2337 2620 2466 2571 2810 -0 2682 1905 2245 2225 2179 2236 1814 1745 1904 1687 1574 1746 1574 1119 2442 939 1586 1060 2064 982 1884 1283 1538 600 1088 786 1076 314 501 0 451 268 734 1033 685 370 876 903 992 2610 1110 1928 1185 2615 1819 1760 913 1990 1398 1289 662 1480 1130 345 940 457 1313 338 1267 404 1084 584 1366 1505 1381 1600 2985 2368 2828 2705 2626 1322 2748 2342 2279 2330 2140 1999 2157 2216 2089 2232 2412 2311 2700 -0 2128 2084 2138 1790 1731 1804 1425 1326 1448 1324 1201 1351 1664 1638 1497 1595 1300 2198 1837 1368 957 840 987 803 572 342 591 530 707 451 0 925 1068 1785 869 1103 1280 1982 1347 2226 1937 2168 2653 2333 1760 1682 2110 1936 1414 1271 1750 1198 1426 1027 1158 1207 1585 1098 1649 1214 1521 1492 1747 1924 1830 1924 2702 2085 2545 2696 2606 2376 2742 2422 2302 2459 2372 2245 1224 2579 2539 2076 2820 2739 2830 -0 2558 2938 3278 2178 2558 2898 2169 2297 2355 1928 1728 2068 1819 1057 2319 1408 3109 1710 3587 778 1761 2261 2564 687 1166 1717 1510 297 585 268 925 0 299 232 723 521 360 509 1002 2569 1642 2992 1885 3426 2068 2569 1399 2879 1618 1725 1090 2544 1157 552 1325 684 1767 253 910 598 1235 895 1434 1060 1218 1689 2944 3454 3914 2443 2953 1532 3374 2527 2595 2734 2158 2255 2527 1992 2109 2547 2078 2183 2473 -0 2283 2663 3003 1903 2283 2623 2266 2378 2516 2050 1507 1847 1165 2191 1987 2742 2451 3345 2929 1690 1670 1986 2288 1342 1175 1453 1694 620 640 734 1068 299 0 761 765 1258 509 622 1213 2353 3068 2777 3985 3211 1941 2353 3014 2664 1455 1659 2511 2329 997 1142 1304 1431 1811 573 817 1411 757 2007 1782 1028 1312 1814 2729 3239 3698 2228 2738 3280 3158 2362 2517 2659 2005 2218 2552 1883 2127 2485 2011 2223 2555 -0 3062 3407 3747 2682 3027 3367 2138 2527 2867 1851 2073 2413 1315 1309 2822 1690 3089 1310 3567 1030 2264 2627 2930 760 1720 2127 2432 463 1282 1033 1785 232 761 0 915 276 557 296 524 4034 1758 3225 1719 3963 3534 2801 1447 3415 2137 2113 1196 2828 1637 652 1587 645 1725 362 1216 442 1053 630 1301 1199 931 1134 4410 3665 4166 3909 3164 1580 3626 3034 2981 2957 2534 2481 2519 2178 2081 2150 2168 2004 2139 -0 3048 2491 2831 2668 2111 2451 2168 1943 1951 1892 1537 1497 1199 2956 2752 2464 2173 3067 2651 2455 2251 1711 2014 1951 1751 1211 1516 674 1062 685 869 723 765 915 0 857 1142 1293 972 3118 2600 2309 4354 3047 2618 1885 3811 2499 1878 1385 3308 1912 1507 1208 1389 1289 1755 852 1502 876 1144 1338 1438 1694 1351 1627 3494 2749 3250 2993 2248 4077 2710 2797 2885 2963 2491 2358 1707 2477 2228 2354 2600 2323 1548 -0 3602 2788 3128 3222 2408 2748 2678 1908 2248 2392 2613 2953 1856 1579 3362 1342 2975 1040 3453 1300 2804 2008 2311 1030 2260 1508 1814 665 1231 370 1103 521 1258 276 857 0 1110 827 369 4575 1553 3765 1211 2652 4074 3342 939 2104 3574 1929 887 1972 3078 563 1500 504 1391 666 1779 372 1257 387 1049 1713 1270 1014 4950 4205 2817 4449 3705 1072 2277 3949 2702 2541 3453 2357 2210 2560 2183 1998 2587 2221 2046 -0 2793 3173 3513 2413 2793 3133 1913 2293 2633 2317 2017 2357 1675 2701 2497 3252 2961 3855 3439 2200 1996 2496 2798 1670 1640 1996 2301 925 920 876 1280 360 509 557 1142 1110 0 462 1040 2863 3578 3287 4242 3721 2363 2863 3271 3174 1946 2075 2768 2839 1463 1494 1655 2082 2123 803 1055 1668 1274 2264 1753 902 1217 1605 3239 3749 4209 2738 3248 3537 3669 2820 2748 3169 2395 2523 2711 2070 2244 2563 1988 2179 2516 -0 3175 3555 3895 2795 3175 3515 2650 2675 3015 2019 2399 2739 1693 3083 2879 3634 3343 2998 3821 2582 2378 2878 3180 1818 1627 2378 2683 1081 1061 903 1982 509 622 296 1293 827 462 0 579 3245 2864 2573 3498 3269 2745 2149 2955 2721 1899 1649 2452 2134 1434 1338 1539 1483 1836 643 1066 1177 964 1303 1336 948 825 1155 3621 3018 3519 3120 2517 3222 2979 2620 2017 2478 2344 2302 2455 2031 1972 2180 1954 1887 2116 -0 3986 3684 3750 3606 3304 3370 3062 2804 2870 2776 2846 3186 2088 3543 3747 3233 3524 2630 3484 2985 3189 2904 2948 2445 2645 2404 2488 1851 2207 992 1347 1002 1213 524 972 369 1040 579 0 3520 3655 2947 2971 2803 3020 2524 2428 2256 2520 1948 1943 2114 1835 1176 1546 1142 1530 1716 1534 757 1204 927 1053 1476 1013 779 3895 3393 3016 3395 2892 2694 2476 2895 2392 2668 2637 2371 2290 2406 2069 1954 2355 1996 1868 -0 2358 2494 2712 2494 2612 2810 2833 2923 2411 2031 2411 2751 3518 1407 1020 1697 1403 1828 2305 1639 1388 1562 1872 2023 1829 1940 2158 2150 2051 2610 2226 2569 2353 4034 3118 4575 2863 3245 3520 0 650 548 1456 1129 525 707 1709 1106 1025 1118 2276 1976 1500 2230 1962 2962 2513 2204 2000 2838 2460 3380 2922 2340 2840 3260 1019 1284 1756 1342 1519 2039 1873 1788 1900 2080 1875 2385 2844 2375 2885 3344 2725 3235 3694 -0 2242 2191 2426 2461 2421 2501 2855 2826 2884 2478 2334 2674 3000 746 1069 634 1012 570 1829 881 1605 1479 1703 1159 2105 1979 1900 1432 2510 1110 1937 1642 3068 1758 2600 1553 3578 2864 3655 650 0 615 416 940 1066 1051 819 1273 1566 1551 865 1959 2259 972 1676 1208 2228 1482 2760 1073 2216 1417 2678 3100 2559 2979 1480 1330 1628 1844 1750 844 1924 2437 1577 2420 2634 2077 2536 3134 2577 3036 3484 2927 3386 -0 1261 881 1221 2752 1261 2767 2918 2873 2929 2481 2043 2383 2709 1012 808 763 1387 1366 2044 1996 1591 1435 1644 2178 1951 1852 1969 2574 2136 1928 2168 2992 2777 3225 2309 3765 3287 2573 2947 548 615 0 812 604 730 503 1518 621 1134 1001 1142 1210 1598 1653 1460 2707 1937 2672 2468 1854 1925 3125 2387 2808 2267 2687 1154 913 1264 1346 1176 1784 1389 1722 1620 1671 2343 2101 2245 2843 2286 2745 3193 2636 3095 -0 2033 2501 1993 2413 2033 1737 2913 2531 2809 3253 2871 2817 4754 827 1859 476 1243 472 1981 1106 2416 1524 1472 1376 2953 2131 2095 1673 3702 1185 2653 1885 3985 1719 4354 1211 4242 3498 2971 1456 416 812 0 588 1944 1153 488 875 2444 1899 581 1403 2944 1022 2360 866 1587 1824 3444 1123 2900 1075 2488 3784 3243 2830 2015 1454 1296 2223 1647 507 1483 2819 1996 1648 3319 2761 2271 3819 3261 2812 4169 3611 3194 -0 1706 1326 1666 2086 1706 1450 2586 2390 2336 2926 2730 2676 3447 1456 1252 2089 916 1514 1394 1929 2283 1237 1088 2425 2451 1990 1954 3008 2653 2615 2333 3426 3211 3963 3047 2652 3721 3269 2803 1129 940 604 588 0 1305 911 762 627 1633 1353 1330 1079 2412 2097 1829 2027 1659 3116 2912 2298 2369 2445 2201 3252 2711 2543 1347 880 783 1545 1188 777 1166 1868 1618 1580 2787 2230 1985 3287 2730 2525 3637 3080 2907 -0 2399 2526 2734 2311 2451 2672 2458 2578 2777 2679 2777 2949 3018 1584 1252 1915 1482 1823 2269 1491 1102 1355 1712 1570 1355 1531 1823 2094 1538 1819 1760 2068 1941 3534 2618 4074 2363 2745 3020 525 1066 730 1944 1305 0 525 1563 990 500 732 1705 1475 1000 1398 1089 2043 1463 1742 1500 2338 1960 2831 2421 1840 2340 2760 1316 1495 1921 1125 1375 1479 1785 1375 1550 1809 1818 1929 2179 1875 2385 2844 2225 2735 3194 -0 2507 2427 2491 2427 2339 2410 2561 2486 2546 2767 2706 2755 2285 1867 1464 1828 1357 1591 1977 1805 1388 1140 1356 1813 1562 1388 1531 2150 1689 1760 1682 2569 2353 2801 1885 3342 2863 2149 2524 707 1051 503 1153 911 525 0 965 470 732 500 1222 1034 1143 1285 960 1543 1265 1772 2000 1685 1501 1791 1963 2340 1844 2264 1464 1284 1542 1342 1087 1167 1326 1519 1342 1416 1900 1788 1879 2375 1862 2321 2725 2212 2671 -0 2266 1914 1574 1886 1534 1194 2386 1988 1934 2726 2328 2274 4211 1116 2437 765 1822 921 1369 1458 1469 2015 2037 1417 2392 1588 2043 1187 2393 913 2110 1399 3014 1447 3811 939 3271 2955 2428 1709 819 1518 488 762 1563 965 0 499 1666 1085 358 600 1804 907 1531 768 1142 1402 2602 990 2103 881 1679 2942 2445 2287 1977 1358 1127 1635 1135 127 747 1790 1290 858 1997 1532 1362 2976 2464 1849 3326 2814 2651 -0 2880 1623 1283 2816 2630 2663 2924 2920 1789 3101 2183 2985 2899 2289 1872 2102 1654 1219 2177 2354 1842 1469 1599 2348 1969 1662 1760 2461 2037 1990 1936 2879 2664 3415 2499 2104 3174 2721 2256 1106 1273 621 875 627 990 470 499 0 1132 748 689 627 1468 1472 1181 1522 1054 2514 2310 1994 1812 1816 1654 2650 2154 1996 1498 1158 1229 1413 977 677 923 1549 1212 1096 1876 1646 1737 2685 2172 1977 3035 2522 2360 -0 2708 2805 2973 2458 2578 2777 2311 2451 2672 2382 2512 2722 2518 2002 1707 2314 1861 2727 2539 1772 1355 1531 1842 1349 1102 1355 1713 1594 1083 1398 1414 1618 1455 2137 1878 3574 1946 1899 2520 1025 1566 1134 2444 1633 500 732 1666 1132 0 525 1205 975 500 986 675 1543 1170 1257 1025 1400 1125 1536 1464 1365 1455 2260 1765 1877 2227 1375 1550 1979 1903 1125 1375 1690 1375 1550 1887 1818 1929 2179 2151 2237 2694 -0 2795 2735 2784 2561 2486 2546 2427 2339 2410 2491 2410 2476 2877 2203 1848 2263 1793 2029 2391 2009 1562 1388 1558 1694 1388 1140 1357 1614 1318 1289 1271 1725 1659 2113 1385 1929 2075 1649 1948 1118 1551 1001 1899 1353 732 500 1085 748 525 0 705 1076 732 790 461 936 731 1289 1118 1198 1000 1280 1463 1430 1340 1404 1848 1736 1944 1519 1342 1365 1515 1342 1087 1220 1519 1342 1489 1900 1788 1879 2209 2122 2193 -0 2870 2365 2311 2490 1985 1931 1990 1485 2627 3014 1825 1771 3708 1345 2895 994 2280 974 2045 1138 2073 1585 1509 886 1573 1085 1616 936 2227 662 1750 1090 2511 1196 3308 887 2768 2452 1943 2276 865 1142 581 1330 1705 1222 358 689 1205 705 0 375 1378 524 829 440 436 1204 1525 638 1707 524 973 2545 1504 1390 2581 1828 1823 2057 1544 484 1337 1580 1309 942 1862 1452 1221 2125 1812 1666 2930 2113 2000 -0 2641 2135 2081 2261 1755 1701 1761 1255 1201 3097 1595 1541 2312 2548 2460 2862 2194 1615 2698 2047 1843 1355 1279 1543 1343 855 1739 2292 1770 1480 1198 2544 2329 2828 1912 1972 2839 2134 2114 1976 1959 1210 1403 1079 1475 1034 600 627 975 1076 375 0 1387 1082 878 975 594 1683 1976 1402 1414 1258 1124 2316 1819 1546 1915 1708 1728 1850 1257 835 1120 1398 937 667 1507 1129 1009 1809 1545 1477 2700 1870 1819 -0 2165 2545 2885 2801 2891 2505 2458 2578 2777 2328 2464 2684 2683 2073 1869 2623 2332 3227 2810 2202 1800 1911 2158 1570 1355 1531 1862 1111 873 1130 1426 1157 997 1637 1507 3078 1463 1434 1835 1500 2259 1598 2944 2412 1000 1143 1804 1468 500 732 1378 1387 0 731 486 1021 1340 800 525 1168 704 1307 1246 865 1002 1270 1875 2385 2845 1818 1929 2479 2199 1375 1550 1809 1125 1375 1771 1375 1550 1887 1679 1805 2078 -0 3008 2390 2730 2628 2010 2350 2477 2354 1850 2283 2216 1350 2252 1282 2768 1181 2071 1223 2549 1162 1896 1762 2004 951 1554 1391 1542 627 1091 345 1027 552 1142 652 1208 563 1494 1338 1176 2230 972 1653 1022 2097 1398 1285 907 1472 986 790 524 1082 731 0 375 151 669 310 857 211 566 291 763 1088 931 960 2605 2093 2552 2221 2091 1037 2177 1842 1650 1662 1624 1350 1474 1707 1473 1575 1917 1513 1815 -0 2627 2121 2461 2842 2797 2853 2527 2466 2542 2516 2350 2566 2541 2534 2331 2094 2229 2697 2281 2323 1890 1793 1941 1765 1513 1357 1576 1368 1150 940 1158 1325 1304 1587 1389 1500 1655 1539 1546 1962 1676 1460 2360 1829 1089 960 1531 1181 675 461 829 878 486 375 0 604 581 854 709 791 540 925 790 992 880 992 2337 2179 2284 1849 1751 1772 1890 1469 1312 1414 1312 1091 1335 1521 1376 1539 1790 1686 1803 -0 3321 2703 2997 2941 2323 2617 2604 1823 2117 2460 2311 1663 2391 1438 3081 1262 2816 961 2731 1159 2523 1923 2195 889 1979 1423 1735 764 1452 457 1207 684 1431 645 1289 504 2082 1483 1142 2962 1208 2707 866 2027 2043 1543 768 1522 1543 936 440 975 1021 151 604 0 427 444 1138 224 814 176 678 2057 1013 925 3093 2580 2072 2488 2080 892 2094 2164 1789 1557 1873 1473 1340 1955 1603 1499 2193 1850 1775 -0 3178 2673 2606 2798 2293 2990 2298 1793 1726 2855 2699 1692 2813 3086 2882 2646 2355 2126 2340 2585 2329 2162 2158 2081 2138 1711 1743 1667 1699 1313 1585 1767 1811 1725 1755 1391 2123 1836 1530 2513 2228 1937 1587 1659 1463 1265 1142 1054 1170 731 436 594 1340 669 581 427 0 1277 1182 854 876 669 541 1397 1073 961 2889 2376 1868 2040 1757 1388 1699 1757 1371 1150 1658 1208 970 1792 1424 1274 1993 1697 1591 -0 2848 3228 3568 2468 2848 3188 2355 2379 2612 2245 2305 2357 2064 1202 2608 1553 3398 1855 3876 923 1896 1900 2853 809 1415 1421 1786 455 852 338 1098 253 573 362 852 666 803 643 1716 2204 1482 2672 1824 3116 1742 1772 1402 2514 1257 1289 1204 1683 800 310 854 444 1277 0 619 442 712 604 1224 829 839 1263 2579 3089 3549 2079 2589 1591 3009 2129 2134 2248 1757 1764 2021 1625 1634 1535 1762 1769 1885 -0 2665 3045 3385 2285 2665 3005 2833 2923 2505 1367 1747 2087 1462 2573 2369 3123 2832 3727 3310 2071 1867 2367 2670 2023 1829 1940 2217 1326 1252 1267 1649 910 817 1216 1502 1779 1055 1066 1534 2000 2760 2468 3444 2912 1500 2000 2602 2310 1025 1118 1525 1976 525 857 709 1138 1182 619 0 1047 460 1366 1000 340 604 980 2375 2885 3345 1875 2385 2979 2805 1788 1900 2080 1342 1519 1858 1087 1342 1741 1228 1434 1800 -0 3755 2591 2931 3375 2211 2551 2610 2534 2051 2488 2766 3106 2098 1655 3515 1282 2272 1323 2750 1376 2957 1811 2113 1008 2413 1576 1616 746 1975 404 1214 598 1411 442 876 372 1668 1177 757 2838 1073 1854 1123 2298 2338 1685 990 1994 1400 1198 638 1402 1168 211 791 224 854 442 1047 0 554 235 573 1099 738 712 3213 2743 3244 2713 2242 1073 2704 2240 2031 1964 2034 1648 1638 1852 1509 1497 1947 1654 1644 -0 3125 2661 3001 2745 2281 2621 2873 2828 2121 1827 2705 1881 2342 3033 2829 2634 2343 3237 2821 2531 2327 1881 2184 2146 1924 1928 2164 1489 1398 1084 1521 1235 757 1053 1144 1257 1274 964 1204 2460 2216 1925 2900 2369 1960 1501 2103 1812 1125 1000 1707 1414 704 566 540 814 876 712 460 554 0 843 541 572 342 572 2835 2365 2866 2335 1864 2744 2326 1883 1789 1972 1495 1343 1538 1310 1089 1334 1407 1230 1427 -0 4350 3207 3415 3970 2827 3035 2881 2327 2535 2800 3362 3702 2485 1953 4111 1478 3247 1176 3149 1674 3553 2427 2613 1404 2031 1927 1883 894 1656 584 1492 895 2007 630 1338 387 2264 1303 927 3380 1417 3125 1075 2445 2831 1791 881 1816 1536 1280 524 1258 1307 291 925 176 669 604 1366 235 843 0 522 1389 960 704 3476 3005 2545 2975 2505 1006 2350 2353 2018 1800 2136 1719 1468 2099 1663 1385 2192 1801 1581 -0 3587 3123 3148 3207 2743 2768 2707 3126 2268 3034 2003 2343 2632 3494 3290 3096 2805 2531 2882 2993 2789 2343 2345 2577 2348 2160 2271 2068 1819 1366 1747 1434 1782 1301 1438 1049 1753 1336 1053 2922 2678 2387 2488 2201 2421 1963 1679 1654 1464 1463 973 1124 1246 763 790 678 541 1224 1000 573 541 522 0 1061 621 422 3297 2826 2366 2796 2326 2052 1826 2041 1776 1644 1881 1373 1240 1619 1174 968 1680 1279 1121 -0 3005 3385 3725 2625 3005 3345 2125 2505 2845 1707 2087 2427 1802 2913 2709 3463 3172 4067 3650 2411 2207 2707 3010 2339 2152 2239 2512 1609 1567 1505 1924 1060 1028 1199 1694 1713 902 948 1476 2340 3100 2808 3784 3252 1840 2340 2942 2650 1365 1430 2545 2316 865 1088 992 2057 1397 829 340 1099 572 1389 1061 0 500 919 2715 3225 3685 2215 2725 3319 3145 2113 2200 2645 1639 1767 2043 1221 1429 1797 1087 1342 1741 -0 3505 3004 3344 3125 2624 2964 2625 2124 2464 2207 2898 2224 2551 3413 3209 2977 2685 3580 3163 2911 2707 2224 2526 2407 2423 2152 2490 1745 1842 1381 1830 1218 1312 931 1351 1270 1217 825 1013 2840 2559 2267 3243 2711 2340 1844 2445 2154 1455 1340 1504 1819 1002 931 880 1013 1073 839 604 738 342 960 621 500 0 419 3215 2713 3214 2715 2212 3000 2674 2200 2113 2300 1767 1639 1744 1429 1221 1395 1342 1087 1302 -0 3925 3424 3490 3545 3044 3110 3045 2544 2610 2627 3065 2644 1546 3833 3629 3397 3105 2890 3224 3331 3127 2644 2688 2827 2627 2144 2519 2414 1935 1600 1924 1689 1814 1134 1627 1014 1605 1155 779 3260 2979 2687 2830 2543 2760 2264 2287 1996 2260 1404 1390 1546 1270 960 992 925 961 1263 980 712 572 704 422 919 419 0 3635 3133 2756 3135 2632 2391 2216 2635 2175 2062 2022 1731 1640 1770 1375 1222 1713 1279 1088 -0 3372 3606 1871 3472 3564 3726 2031 2411 2751 2371 2751 3091 3894 2415 2039 2756 2422 2037 3134 2609 2316 2449 2731 2883 2672 2769 2982 2526 2170 2985 2702 2944 2729 4410 3494 4950 3239 3621 3895 1019 1480 1154 2015 1347 1316 1464 1977 1498 1765 1848 2581 1915 1875 2605 2337 3093 2889 2579 2375 3213 2835 3476 3297 2715 3215 3635 0 500 1001 545 752 1456 1127 1045 1163 1358 1500 2010 2469 2000 2510 2969 2350 2860 3319 -0 1575 1195 1535 3524 3466 3512 2455 2143 2483 2795 2483 2823 3149 2676 2265 2591 2278 2364 2941 2791 2449 2316 2477 3005 2769 2672 2780 3036 2680 2368 2085 3454 3239 3665 2749 4205 3749 3018 3393 1284 1330 913 1454 880 1495 1284 1358 1158 1877 1736 1828 1708 2385 2093 2179 2580 2376 3089 2885 2743 2365 3005 2826 3225 2713 3133 500 0 503 752 545 1021 734 1163 1045 1084 2000 1500 1959 2500 2000 2459 2850 2350 2809 -0 1998 1618 1958 2378 3869 1766 2878 2602 2470 3218 2942 2810 3650 1807 2731 2892 1208 2245 3114 3294 2829 2542 2733 2912 2712 2874 3041 3496 3140 2828 2545 3914 3698 4166 3250 2817 4209 3519 3016 1756 1628 1264 1296 783 1921 1542 1127 1229 2227 1944 1823 1728 2845 2552 2284 2072 1868 3549 3345 3244 2866 2545 2366 3685 3214 2756 1001 503 0 1101 719 881 539 1423 1156 1120 2424 1924 1543 2924 2424 2043 3274 2774 2393 -0 3495 3588 3753 3433 3532 3703 3537 3628 2380 3708 2380 2720 3393 2620 2267 3021 2472 2602 2185 2604 2227 2380 2684 2610 2380 2510 2774 2996 2458 2705 2696 2443 2228 3909 2993 4449 2738 3120 3395 1342 1844 1346 2223 1545 1125 1342 1635 1413 1375 1519 2057 1850 1818 2221 1849 2488 2040 2079 1875 2713 2335 2975 2796 2215 2715 3135 545 752 1101 0 500 1466 955 500 707 1092 1000 1118 1379 1500 2010 2469 1850 2360 2819 -0 3547 3489 3536 3489 3427 3477 3588 3532 3577 3753 3703 3901 2648 2812 2401 2882 2359 2098 3071 2793 2380 2227 2403 2768 2510 2380 2520 2887 2564 2626 2606 2953 2738 3164 2248 3705 3248 2517 2892 1519 1750 1176 1647 1188 1375 1087 1135 977 1550 1342 1544 1257 1929 2091 1751 2080 1757 2589 2385 2242 1864 2505 2326 2725 2212 2632 752 545 719 500 0 966 455 707 500 698 1118 1000 1096 2010 1500 1959 2360 1850 2309 -0 2644 2180 1840 2264 1800 1460 2764 2237 2200 3104 2577 2540 4477 1249 2619 898 2088 957 1635 1500 2578 2148 2181 1509 2743 2327 2268 1320 2997 1322 2376 1532 3280 1580 4077 1072 3537 3222 2694 2039 844 1784 507 777 1479 1167 127 677 1979 1365 484 835 2479 1037 1772 892 1388 1591 2979 1073 2744 1006 2052 3319 3000 2391 1456 1021 881 1466 966 0 579 1536 1119 828 2104 1504 1348 2604 2094 1677 2954 2444 2027 -0 3779 1946 1606 3729 3677 1226 3815 2062 1930 2930 4017 2270 3110 3136 2710 3099 2520 1542 3132 3127 2713 2388 2556 3186 2813 2521 2730 2956 2828 2748 2742 3374 3158 3626 2710 2277 3669 2979 2476 1873 1924 1389 1483 1166 1785 1326 747 923 1903 1515 1337 1120 2199 2177 1890 2094 1699 3009 2805 2704 2326 2350 1826 3145 2674 2216 1127 734 539 955 455 579 0 1082 688 581 1393 1119 1035 2430 1920 1503 2780 2270 1853 -0 3732 2420 2760 3537 3628 2380 3433 3532 3703 3483 3577 3743 2893 2949 2627 2498 2782 3102 2685 2793 2380 2510 2790 2471 2227 2380 2684 2515 2109 2342 2422 2527 2362 3034 2797 3949 2820 2620 2895 1788 2437 1722 2819 1868 1375 1519 1790 1549 1125 1342 1580 1398 1375 1842 1469 2164 1757 2129 1788 2240 1883 2353 2041 2113 2200 2635 1045 1163 1423 500 707 1536 1082 0 500 965 500 707 1073 1000 1118 1379 1350 1439 2319 -0 3776 3727 3766 3588 3532 3577 3489 3427 3477 3536 3477 3524 2148 3098 2725 3171 2699 2598 3271 2954 2510 2380 2540 2651 2380 2227 2403 2551 2255 2279 2302 2595 2517 2981 2885 2702 2748 2017 2392 1900 1577 1620 1996 1618 1550 1342 1290 1212 1375 1087 1309 937 1550 1650 1312 1789 1371 2134 1900 2031 1789 2018 1776 2200 2113 2175 1163 1045 1156 707 500 1119 688 500 0 465 707 500 672 1118 1000 1096 1439 1350 1423 -0 2970 2443 2308 3870 3729 1928 3697 3545 1428 3798 3658 1768 2609 2878 2674 2416 2725 1842 3217 3257 2710 2412 2598 2942 2608 2345 2403 2744 2476 2330 2459 2734 2659 2957 2963 2541 3169 2478 2668 2080 2420 1671 1648 1580 1809 1416 858 1096 1690 1220 942 667 1809 1662 1414 1557 1150 2248 2080 1964 1972 1800 1644 2645 2300 2062 1358 1084 1120 1092 698 828 581 965 465 0 1092 698 546 1404 1129 1045 2280 1459 1395 -0 2540 2920 3260 2160 2540 2880 3537 3628 2380 3444 1622 1962 2397 2448 2244 2998 2707 3602 3185 3143 2730 2827 2545 2610 2380 2510 2807 2231 1992 2140 2372 2158 2005 2534 2491 3453 2395 2344 2637 1875 2634 2343 3319 2787 1818 1900 1997 1876 1375 1519 1862 1507 1125 1624 1312 1873 1658 1757 1342 2034 1495 2136 1881 1639 1767 2022 1500 2000 2424 1000 1118 2104 1393 500 707 1092 0 500 950 500 707 1073 850 986 1274 -0 3050 2522 2862 2670 2142 2482 3588 3532 3577 1752 3438 1507 1648 2958 2754 2495 2204 3098 2682 2456 2827 2730 2869 2768 2510 2380 2561 2383 2158 1999 2245 2255 2218 2481 2358 2357 2523 2302 2371 2385 2077 2101 2761 2230 1929 1788 1532 1646 1550 1342 1452 1129 1375 1350 1091 1473 1208 1764 1519 1648 1343 1719 1373 1767 1639 1731 2010 1500 1924 1118 1000 1504 1119 707 500 698 500 0 450 707 500 672 986 850 961 -0 3509 2981 2931 3129 2601 2551 3783 2101 2051 3706 1626 1966 2107 3417 3213 2954 2663 2451 2665 2915 2711 2201 3009 3054 2790 2486 2630 2693 2485 2157 1224 2527 2552 2519 1707 2210 2711 2455 2290 2844 2536 2245 2271 1985 2179 1879 1362 1737 1887 1489 1221 1009 1771 1474 1335 1340 970 2021 1858 1638 1538 1468 1240 2043 1744 1640 2469 1959 1543 1379 1096 1348 1035 1073 672 546 950 450 0 1073 672 500 1274 961 850 -0 3040 3420 3760 2660 3040 3380 2160 2540 2880 1742 2122 2462 3677 2948 2744 3498 3207 4102 3685 2446 2242 2742 3045 2942 2730 2827 2547 2338 2205 2216 2579 1992 1883 2178 2477 2560 2070 2031 2406 2375 3134 2843 3819 3287 1875 2375 2976 2685 1818 1900 2125 1809 1375 1707 1521 1955 1792 1625 1087 1852 1310 2099 1619 1221 1429 1770 2000 2500 2924 1500 2010 2604 2430 1000 1118 1404 500 707 1073 0 500 950 350 610 1012 -0 3550 3022 3362 3170 2642 2982 2670 2142 2482 2252 1906 2246 3428 3458 3254 2995 2704 3598 3182 2956 2752 2242 2545 3063 2827 2730 3022 2473 2337 2089 2539 2109 2127 2081 2228 2183 2244 1972 2069 2885 2577 2286 3261 2730 2385 1862 2464 2172 1929 1788 1812 1545 1550 1473 1376 1603 1424 1634 1342 1509 1089 1663 1174 1429 1221 1375 2510 2000 2424 2010 1500 2094 1920 1118 1000 1129 707 500 672 500 0 450 610 350 570 -0 4009 3481 3472 3629 3101 3092 3129 2601 2592 2711 2365 2705 1607 3917 3713 3454 3163 2855 3206 3415 3211 2701 2669 2911 2711 2201 3058 2883 2620 2232 2076 2547 2485 2150 2354 1998 2563 2180 1954 3344 3036 2745 2812 2525 2844 2321 1849 1977 2179 1879 1666 1477 1887 1575 1539 1499 1274 1535 1741 1497 1334 1385 968 1797 1395 1222 2969 2459 2043 2469 1959 1677 1503 1379 1096 1045 1073 672 500 950 450 0 1012 570 350 -0 3390 3770 4110 3010 3390 3730 2510 2890 3230 2092 2472 2812 2175 3298 3094 3848 3557 4452 4035 2796 2592 3092 3395 2292 2092 2592 2897 2554 2466 2412 2820 2078 2011 2168 2600 2587 1988 1954 2355 2725 3484 3193 4169 3637 2225 2725 3326 3035 2151 2209 2930 2700 1679 1917 1790 2193 1993 1762 1228 1947 1407 2192 1680 1087 1342 1713 2350 2850 3274 1850 2360 2954 2780 1350 1439 2280 850 986 1274 350 610 1012 0 500 950 -0 3900 3372 3712 3520 2992 3332 3020 2492 2832 2602 2246 2586 3632 3808 3604 3345 3054 3948 3532 3306 3102 2592 2895 2802 2602 2092 2397 2665 2571 2311 2739 2183 2223 2004 2323 2221 2179 1887 1996 3235 2927 2636 3611 3080 2735 2212 2814 2522 2237 2122 2113 1870 1805 1513 1686 1850 1697 1769 1434 1654 1230 1801 1279 1342 1087 1279 2860 2350 2774 2360 1850 2444 2270 1439 1350 1459 986 850 961 610 350 570 500 0 450 -0 4359 3831 3854 3979 3451 3474 3479 2951 2974 3061 2705 3045 1948 4267 4063 3804 3513 3254 3588 3765 3561 3051 3051 3261 3061 2551 2591 2915 2810 2700 2830 2473 2555 2139 1548 2046 2516 2116 1868 3694 3386 3095 3194 2907 3194 2671 2651 2360 2694 2193 2000 1819 2078 1815 1803 1775 1591 1885 1800 1644 1427 1581 1121 1741 1302 1088 3319 2809 2393 2819 2309 2027 1853 2319 1423 1395 1274 961 850 1012 570 350 950 450 0 +0 200723 201078 144 200811 201183 +0 0 341 1281 972 697 +0 341 0 1828 1314 500 +0 1281 1828 0 186 699 +0 972 1314 186 0 1248 +0 697 500 699 1248 0 EOF \ No newline at end of file diff --git a/falcon_planner/exploration_utils/resource/coverage_path.txt b/falcon_planner/exploration_utils/resource/coverage_path.txt index e7bb1e1..6017bdc 100644 --- a/falcon_planner/exploration_utils/resource/coverage_path.txt +++ b/falcon_planner/exploration_utils/resource/coverage_path.txt @@ -1,94 +1,14 @@ -NAME : coverage_path.38284.tour -COMMENT : Length = 38284 -COMMENT : Found by LKH [Keld Helsgaun] Sat Dec 7 15:54:38 2024 +NAME : coverage_path.2143.tour +COMMENT : Length = 2143 +COMMENT : Found by LKH [Keld Helsgaun] Thu Dec 12 14:15:52 2024 TYPE : TOUR -DIMENSION : 86 +DIMENSION : 6 TOUR_SECTION 1 -42 -44 -45 -70 -69 -68 -71 -75 -78 -81 -84 -85 -86 -83 -82 -79 -80 -77 -76 -72 -74 -73 -48 -52 -53 -49 -47 -43 -41 -46 -50 -51 -56 -54 -60 -65 -62 -66 -67 -64 -58 -57 -63 -61 -55 -59 -33 -34 -38 -39 -35 -37 -40 -36 -29 -31 -30 -32 -27 -28 -24 -23 -22 -26 -25 -21 -15 -16 -18 -19 -17 -20 4 -7 -6 -3 -2 5 -8 -11 -12 -13 -10 -9 -14 +2 +3 +6 -1 EOF diff --git a/falcon_planner/fast_planner/src/planner_manager.cpp b/falcon_planner/fast_planner/src/planner_manager.cpp index 2544f09..b5c3541 100644 --- a/falcon_planner/fast_planner/src/planner_manager.cpp +++ b/falcon_planner/fast_planner/src/planner_manager.cpp @@ -84,32 +84,54 @@ void FastPlannerManager::planExplorationPositionTraj(const vector 1 - 1e-3) { + is_tour_straight_line = true; + } + } + if (is_tour_straight_line) { + PolynomialTraj::oneSegmentTraj(tour[0], tour[2], cur_vel_bound, Eigen::Vector3d::Zero(), + cur_acc_bound, Eigen::Vector3d::Zero(), times.sum(), local_data_.init_traj_poly_); + } else { + PolynomialTraj::waypointsTraj(pos, cur_vel_bound, Eigen::Vector3d::Zero(), cur_acc_bound, + Eigen::Vector3d::Zero(), times, local_data_.init_traj_poly_); + } PolynomialTraj &init_traj = local_data_.init_traj_poly_; if (tour.size() == 3 && (tour[1] - tour[0]).norm() + (tour[2] - tour[1]).norm() < 1.0) { ROS_WARN("[FastPlannerManager] Short path, no need to optimize."); need_optimize = false; + PolynomialTraj::oneSegmentTraj(tour[0], tour[2], cur_vel_bound, Eigen::Vector3d::Zero(), + cur_acc_bound, Eigen::Vector3d::Zero(), times.sum(), init_traj); + // Hack for short tour glitch double tour_len = (tour[1] - tour[0]).norm() + (tour[2] - tour[1]).norm(); if (init_traj.getLength() > 5.0 * tour_len) { @@ -298,9 +336,8 @@ void FastPlannerManager::planExplorationYawWaypointsTraj( // Call B-spline optimization solver int cost_func = BsplineOptimizer::EXPLORATION_YAW_PHASE; - vector start = {Eigen::Vector3d(start_yaw3d[0], cur_yaw_vel, 0), - Eigen::Vector3d(start_yaw3d[1], 0, 0), - Eigen::Vector3d(start_yaw3d[2], 0, 0)}; + vector start = {Eigen::Vector3d(start_yaw3d[0], start_yaw3d[1], start_yaw3d[2]), + Eigen::Vector3d(cur_yaw_vel, 0, 0), Eigen::Vector3d(0, 0, 0)}; vector end = {Eigen::Vector3d(end_yaw3d[0], 0, 0), Eigen::Vector3d(0, 0, 0)}; bspline_optimizers_[1]->setBoundaryStates(start, end); bspline_optimizers_[1]->setWaypoints(yaw_waypts, yaw_waypts_idx); diff --git a/falcon_planner/trajectory/include/polynomial/polynomial_traj.h b/falcon_planner/trajectory/include/polynomial/polynomial_traj.h index 4e796d0..4eb6379 100644 --- a/falcon_planner/trajectory/include/polynomial/polynomial_traj.h +++ b/falcon_planner/trajectory/include/polynomial/polynomial_traj.h @@ -154,6 +154,11 @@ class PolynomialTraj { mean_d /= double(sample_num); } + static void oneSegmentTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &end_pos, + const Eigen::Vector3d &start_vel, const Eigen::Vector3d &end_vel, + const Eigen::Vector3d &start_acc, const Eigen::Vector3d &end_acc, + const double &time, PolynomialTraj &poly_traj); + // input : position of waypoints, start/end vel and acc, segment time // Pos: Nx3 static void waypointsTraj(const Eigen::MatrixXd &positions, const Eigen::Vector3d &start_vel, diff --git a/falcon_planner/trajectory/src/polynomial/polynomial_traj.cpp b/falcon_planner/trajectory/src/polynomial/polynomial_traj.cpp index 5fc5058..dfd0d42 100644 --- a/falcon_planner/trajectory/src/polynomial/polynomial_traj.cpp +++ b/falcon_planner/trajectory/src/polynomial/polynomial_traj.cpp @@ -2,6 +2,42 @@ #include namespace fast_planner { +void PolynomialTraj::oneSegmentTraj(const Eigen::Vector3d &start_pos, + const Eigen::Vector3d &end_pos, + const Eigen::Vector3d &start_vel, + const Eigen::Vector3d &end_vel, + const Eigen::Vector3d &start_acc, + const Eigen::Vector3d &end_acc, const double &time, + PolynomialTraj &poly_traj) { + Eigen::MatrixXd Ab = Eigen::MatrixXd::Zero(6, 6); + Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6); + Eigen::VectorXd Dy = Eigen::VectorXd::Zero(6); + Eigen::VectorXd Dz = Eigen::VectorXd::Zero(6); + double T = time; + // clang-format off + Ab << 1, 0, 0, 0, 0, 0, \ + 1, pow(T, 1), pow(T, 2), pow(T, 3), pow(T, 4), pow(T, 5), \ + 0, 1, 0, 0, 0, 0, \ + 0, 1, 2 * pow(T, 1), 3 * pow(T, 2), 4 * pow(T, 3), 5 * pow(T, 4), \ + 0, 0, 2, 0, 0, 0, \ + 0, 0, 2, 6 * pow(T, 1), 12 * pow(T, 2), 20 * pow(T, 3); + // clang-format on + Dx << start_pos(0), end_pos(0), start_vel(0), end_vel(0), start_acc(0), end_acc(0); + Dy << start_pos(1), end_pos(1), start_vel(1), end_vel(1), start_acc(1), end_acc(1); + Dz << start_pos(2), end_pos(2), start_vel(2), end_vel(2), start_acc(2), end_acc(2); + + Eigen::VectorXd Px, Py, Pz; + Px = Eigen::VectorXd::Zero(6); + Py = Eigen::VectorXd::Zero(6); + Pz = Eigen::VectorXd::Zero(6); + Px = Ab.colPivHouseholderQr().solve(Dx); + Py = Ab.colPivHouseholderQr().solve(Dy); + Pz = Ab.colPivHouseholderQr().solve(Dz); + poly_traj.reset(); + Polynomial poly(Px, Py, Pz, time); + poly_traj.addSegment(poly); +} + void PolynomialTraj::waypointsTraj(const Eigen::MatrixXd& positions, const Eigen::Vector3d& start_vel, const Eigen::Vector3d& end_vel, const Eigen::Vector3d& start_acc, const Eigen::Vector3d& end_acc, const Eigen::VectorXd& times, From 312eb4d32c6c7af1a482f94a0a204aa2bb150cca Mon Sep 17 00:00:00 2001 From: Chen Xinyi Date: Fri, 16 May 2025 15:42:43 +0800 Subject: [PATCH 2/2] Update README.md --- README.md | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 5ec7f60..2970e9d 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,10 @@ This is the code repository for the paper contributed to IEEE T-RO: -*FALCON: Fast Autonomous Aerial Exploration using Coverage Path Guidance*. + +**[FALCON: Fast Autonomous Aerial Exploration using Coverage Path Guidance](https://ieeexplore.ieee.org/document/10816079)** + +**Authors**: [Yichen Zhang](https://yzhangec.github.io/)\*, [Xinyi Chen](https://xchencq.github.io/)\*, [Chen Feng](https://chen-albert-feng.github.io/AlbertFeng.github.io/), [Boyu Zhou](https://robotics-star.com/people), and [Shaojie Shen](https://uav.hkust.edu.hk/group/). This paper introduces FALCON, a Fast Autonomous aerial robot expLoration planner using COverage path guidaNce. FALCON effectively harnesses the full potential of online generated coverage paths in enhancing exploration efficiency. We also introduce a lightweight exploration planner evaluation environment that allows for comparing exploration planners across a variety of testing scenarios using an identical quadrotor simulator. @@ -23,14 +26,16 @@ This paper introduces FALCON, a Fast Autonomous aerial robot expLoration planner Please cite our paper if you use this project in your research: -Zhang, Y., Chen, X., Feng, C., Zhou, B., & Shen, S. (2024). FALCON: Fast Autonomous Aerial Exploration using Coverage Path Guidance. arXiv preprint arXiv:2407.00577. - +Zhang, Y., Chen, X., Feng, C., Zhou, B., & Shen, S. (2024). FALCON: Fast Autonomous Aerial Exploration using Coverage Path Guidance. IEEE Transactions on Robotics. ``` @article{zhang2024falcon, - title={FALCON: Fast Autonomous Aerial Exploration using Coverage Path Guidance}, - author={Yichen Zhang, Xinyi Chen, Chen Feng, Boyu Zhou, Shaojie Shen}, - journal={arXiv preprint arXiv:2407.00577}, - year={2024} + title={FALCON: Fast Autonomous Aerial Exploration Using Coverage Path Guidance}, + author={Zhang, Yichen and Chen, Xinyi and Feng, Chen and Zhou, Boyu and Shen, Shaojie}, + journal={IEEE Transactions on Robotics}, + year={2024}, + volume={41}, + pages={1365-1385}, + doi={10.1109/TRO.2024.3522148} } ``` @@ -165,4 +170,4 @@ It is recommended to use the [UniQuad](https://github.com/HKUST-Aerial-Robotics/ ## To be included in the future - [ ] Lidar sensing from [MARSIM](https://github.com/hku-mars/MARSIM) - [ ] Large-scale scenario (> 1e6 m^3) compatibility -- [ ] ROS2 support \ No newline at end of file +- [ ] ROS2 support