search_for_calc.cpp

本文介绍了一个简单的Windows应用程序示例,展示了如何使用Win32 API进行窗口注册、创建及消息处理,并针对不同版本的Windows进行了适配。

  name="google_ads_frame" marginwidth="0" marginheight="0" src="http://pagead2.googlesyndication.com/pagead/ads?client=ca-pub-5572165936844014&dt=1194442938015&lmt=1194190197&format=336x280_as&output=html&correlator=1194442937843&url=file%3A%2F%2F%2FC%3A%2FDocuments%2520and%2520Settings%2Flhh1%2F%E6%A1%8C%E9%9D%A2%2FCLanguage.htm&color_bg=FFFFFF&color_text=000000&color_link=000000&color_url=FFFFFF&color_border=FFFFFF&ad_type=text&ga_vid=583001034.1194442938&ga_sid=1194442938&ga_hid=1942779085&flash=9&u_h=768&u_w=1024&u_ah=740&u_aw=1024&u_cd=32&u_tz=480&u_java=true" frameborder="0" width="336" scrolling="no" height="280" allowtransparency="allowtransparency"> 
#include <windows.h> 
#include "Search_For_Calc.h"


#if defined (WIN32)
 #define IS_WIN32 TRUE
#else
 #define IS_WIN32 FALSE
#endif

#define IS_NT      IS_WIN32 && (BOOL)(GetVersion() < 0x80000000)
#define IS_WIN32S  IS_WIN32 && (BOOL)(!(IS_NT) && (LOBYTE(LOWORD(GetVersion()))<4))
#define IS_WIN95   (BOOL)(!(IS_NT) && !(IS_WIN32S)) && IS_WIN32

HINSTANCE hInst;   // current instance

LPCTSTR lpszAppName  = "MyApp";
LPCTSTR lpszTitle    = "My Application";

BOOL RegisterWin95( CONST WNDCLASS* lpwc );

int APIENTRY WinMain( HINSTANCE hInstance, HINSTANCE hPrevInstance,
                      LPTSTR lpCmdLine, int nCmdShow)
{
   MSG      msg;
   HWND     hWnd;
   WNDCLASS wc;

   // Register the main application window class.
   //............................................
   wc.style         = CS_HREDRAW | CS_VREDRAW;
   wc.lpfnWndProc   = (WNDPROC)WndProc;      
   wc.cbClsExtra    = 0;                     
   wc.cbWndExtra    = 0;                     
   wc.hInstance     = hInstance;             
   wc.hIcon         = LoadIcon( hInstance, lpszAppName );
   wc.hCursor       = LoadCursor(NULL, IDC_ARROW);
   wc.hbrBackground = (HBRUSH)(COLOR_WINDOW+1);
   wc.lpszMenuName  = lpszAppName;             
   wc.lpszClassName = lpszAppName;             

   if ( IS_WIN95 )
   {
      if ( !RegisterWin95( &wc ) )
         return( FALSE );
   }
   else if ( !RegisterClass( &wc ) )
      return( FALSE );

   hInst = hInstance;

   // Create the main application window.
   //....................................
   hWnd = CreateWindow( lpszAppName,
                        lpszTitle,   
                        WS_OVERLAPPEDWINDOW,
                        CW_USEDEFAULT, 0,
                        CW_USEDEFAULT, 0, 
                        NULL,             
                        NULL,             
                        hInstance,        
                        NULL              
                      );

   if ( !hWnd )
      return( FALSE );

   ShowWindow( hWnd, nCmdShow );
   UpdateWindow( hWnd );        

   while( GetMessage( &msg, NULL, 0, 0) )  
   {
      TranslateMessage( &msg );
      DispatchMessage( &msg ); 
   }

   return( msg.wParam );
}


BOOL RegisterWin95( CONST WNDCLASS* lpwc )
{
   WNDCLASSEX wcex;

   wcex.style         = lpwc->style;
   wcex.lpfnWndProc   = lpwc->lpfnWndProc;
   wcex.cbClsExtra    = lpwc->cbClsExtra;
   wcex.cbWndExtra    = lpwc->cbWndExtra;
   wcex.hInstance     = lpwc->hInstance;
   wcex.hIcon         = lpwc->hIcon;
   wcex.hCursor       = lpwc->hCursor;
   wcex.hbrBackground = lpwc->hbrBackground;
   wcex.lpszMenuName  = lpwc->lpszMenuName;
   wcex.lpszClassName = lpwc->lpszClassName;

   // Added elements for Windows 95.
   //...............................
   wcex.cbSize = sizeof(WNDCLASSEX);
   wcex.hIconSm = LoadImage(wcex.hInstance, lpwc->lpszClassName,
                            IMAGE_ICON, 16, 16,
                            LR_DEFAULTCOLOR );
   
   return RegisterClassEx( &wcex );
}

LRESULT CALLBACK WndProc( HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam )
{
   switch( uMsg )
   {
      case WM_COMMAND :
              switch( LOWORD( wParam ) )
              {
                 case IDM_TEST :
                        {
                           char   szBuf[MAX_PATH];
                           LPTSTR lpFile;

                           if ( SearchPath( NULL, "CALC.EXE", NULL,
                                            MAX_PATH, szBuf, &lpFile ) )
                           {
                              MessageBox( hWnd, szBuf, "Found", MB_OK );
                           }
                           else
                              MessageBox( hWnd, "Could not find CALC.EXE",
                                          "Not Found", MB_OK );
                        }
                        break;

                 case IDM_ABOUT :
                        DialogBox( hInst, "AboutBox", hWnd, (DLGPROC)About );
                        break;

                 case IDM_EXIT :
                        DestroyWindow( hWnd );
                        break;
              }
              break;
     
      case WM_DESTROY :
              PostQuitMessage(0);
              break;

      default :
            return( DefWindowProc( hWnd, uMsg, wParam, lParam ) );
   }

   return( 0L );
}


LRESULT CALLBACK About( HWND hDlg,          
                        UINT message,       
                        WPARAM wParam,      
                        LPARAM lParam)
{
   switch (message)
   {
       case WM_INITDIALOG:
               return (TRUE);

       case WM_COMMAND:                             
               if (   LOWORD(wParam) == IDOK        
                   || LOWORD(wParam) == IDCANCEL)   
               {
                       EndDialog(hDlg, TRUE);       
                       return (TRUE);
               }
               break;
   }

   return (FALSE);
}

Downloading https://pypi.tuna.tsinghua.edu.cn/packages/a1/d6/8422797e35f8814b1d9842530566a949d9b5850a466321a6c1d5a99055ee/opencv-python-4.3.0.38.tar.gz (88.0 MB) ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ 88.0/88.0 MB 11.0 MB/s 0:00:08 Installing build dependencies ... error error: subprocess-exited-with-error × pip subprocess to install build dependencies did not run successfully. │ exit code: 1 ╰─> [361 lines of output] Looking in indexes: https://pypi.tuna.tsinghua.edu.cn/simple Ignoring numpy: markers 'python_version == "3.5"' don't match your environment Ignoring numpy: markers 'python_version == "3.6"' don't match your environment Ignoring numpy: markers 'python_version == "3.7"' don't match your environment Collecting setuptools Downloading https://pypi.tuna.tsinghua.edu.cn/packages/a3/dc/17031897dae0efacfea57dfd3a82fdd2a2aeb58e0ff71b77b87e44edc772/setuptools-80.9.0-py3-none-any.whl (1.2 MB) ---------------------------------------- 1.2/1.2 MB 1.7 MB/s 0:00:00 Collecting wheel Downloading https://pypi.tuna.tsinghua.edu.cn/packages/0b/2c/87f3254fd8ffd29e4c02732eee68a83a1d3c346ae39bc6822dcbcb697f2b/wheel-0.45.1-py3-none-any.whl (72 kB) Collecting scikit-build Downloading https://pypi.tuna.tsinghua.edu.cn/packages/c3/a3/21b519f58de90d684056c52ec4e45f744cfda7483f082dcc4dd18cc74a93/scikit_build-0.18.1-py3-none-any.whl (85 kB) Collecting cmake Downloading https://pypi.tuna.tsinghua.edu.cn/packages/b0/98/768e4b298a5a53cd305f7aadae2cc825f00ec70a3b1b98d822c7feacb50b/cmake-4.1.0-py3-none-win32.whl (34.3 MB) ---------------------------------------- 34.3/34.3 MB 12.4 MB/s 0:00:02 Collecting pip Downloading https://pypi.tuna.tsinghua.edu.cn/packages/b7/3f/945ef7ab14dc4f9d7f40288d2df998d1837ee0888ec3659c813487572faa/pip-25.2-py3-none-any.whl (1.8 MB) ---------------------------------------- 1.8/1.8 MB 11.9 MB/s 0:00:00 Collecting numpy==1.17.3 Downloading https://pypi.tuna.tsinghua.edu.cn/packages/b6/d6/be8f975f5322336f62371c9abeb936d592c98c047ad63035f1b38ae08efe/numpy-1.17.3.zip (6.4 MB) ---------------------------------------- 6.4/6.4 MB 13.0 MB/s 0:00:00 Preparing metadata (setup.py): started Preparing metadata (setup.py): finished with status 'done' Collecting distro (from scikit-build) Downloading https://pypi.tuna.tsinghua.edu.cn/packages/12/b3/231ffd4ab1fc9d679809f356cebee130ac7daa00d6d6f3206dd4fd137e9e/distro-1.9.0-py3-none-any.whl (20 kB) Collecting packaging (from scikit-build) Using cached https://pypi.tuna.tsinghua.edu.cn/packages/20/12/38679034af332785aac8774540895e234f4d07f7545804097de4b666afd8/packaging-25.0-py3-none-any.whl (66 kB) Collecting tomli (from scikit-build) Downloading https://pypi.tuna.tsinghua.edu.cn/packages/6e/c2/61d3e0f47e2b74ef40a68b9e6ad5984f6241a942f7cd3bbfbdbd03861ea9/tomli-2.2.1-py3-none-any.whl (14 kB) Building wheels for collected packages: numpy DEPRECATION: Building 'numpy' using the legacy setup.py bdist_wheel mechanism, which will be removed in a future version. pip 25.3 will enforce this behaviour change. A possible replacement is to use the standardized build interface by setting the `--use-pep517` option, (possibly combined with `--no-build-isolation`), or adding a `pyproject.toml` file to the source tree of 'numpy'. Discussion can be found at https://github.com/pypa/pip/issues/6334 Building wheel for numpy (setup.py): started Building wheel for numpy (setup.py): finished with status 'error' error: subprocess-exited-with-error python setup.py bdist_wheel did not run successfully. exit code: 1 [292 lines of output] Running from numpy source directory. blas_opt_info: blas_mkl_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries mkl_rt not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE blis_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries blis not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE openblas_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries openblas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] get_default_fcompiler: matching types: '['gnu', 'intelv', 'absoft', 'compaqv', 'intelev', 'gnu95', 'g95', 'intelvem', 'intelem', 'flang']' customize GnuFCompiler Could not locate executable g77 Could not locate executable f77 customize IntelVisualFCompiler Could not locate executable ifort Could not locate executable ifl customize AbsoftFCompiler Could not locate executable f90 customize CompaqVisualFCompiler Could not locate executable DF customize IntelItaniumVisualFCompiler Could not locate executable efl customize Gnu95FCompiler Could not locate executable gfortran Could not locate executable f95 customize G95FCompiler Could not locate executable g95 customize IntelEM64VisualFCompiler customize IntelEM64TFCompiler Could not locate executable efort Could not locate executable efc customize PGroupFlangCompiler Could not locate executable flang don't know how to compile Fortran code on platform 'nt' NOT AVAILABLE atlas_3_10_blas_threads_info: Setting PTATLAS=ATLAS No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries tatlas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE atlas_3_10_blas_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries satlas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE atlas_blas_threads_info: Setting PTATLAS=ATLAS No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries ptf77blas,ptcblas,atlas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE atlas_blas_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries f77blas,cblas,atlas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE accelerate_info: NOT AVAILABLE C:\Users\Administrator\AppData\Local\Temp\pip-install-hgll2ytb\numpy_4b57488625214bf0a91e82d3a36ac7de\numpy\distutils\system_info.py:690: UserWarning: Optimized (vendor) Blas libraries are not found. Falls back to netlib Blas library which has worse performance. A better performance should be easily gained by switching Blas library. self.calc_info() blas_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries blas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE C:\Users\Administrator\AppData\Local\Temp\pip-install-hgll2ytb\numpy_4b57488625214bf0a91e82d3a36ac7de\numpy\distutils\system_info.py:690: UserWarning: Blas (http://www.netlib.org/blas/) libraries not found. Directories to search for the libraries can be specified in the numpy/distutils/site.cfg file (section [blas]) or by setting the BLAS environment variable. self.calc_info() blas_src_info: NOT AVAILABLE C:\Users\Administrator\AppData\Local\Temp\pip-install-hgll2ytb\numpy_4b57488625214bf0a91e82d3a36ac7de\numpy\distutils\system_info.py:690: UserWarning: Blas (http://www.netlib.org/blas/) sources not found. Directories to search for the sources can be specified in the numpy/distutils/site.cfg file (section [blas_src]) or by setting the BLAS_SRC environment variable. self.calc_info() NOT AVAILABLE 'svnversion' 不是内部或外部命令,也不是可运行的程序 或批处理文件。 non-existing path in 'numpy\\distutils': 'site.cfg' lapack_opt_info: lapack_mkl_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries mkl_rt not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE openblas_lapack_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries openblas not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE openblas_clapack_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries openblas,lapack not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE flame_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries flame not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE atlas_3_10_threads_info: Setting PTATLAS=ATLAS No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries tatlas,tatlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries tatlas,tatlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries tatlas,tatlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\Library\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries tatlas,tatlas not found in E:\Anaconda3\Library\lib <class 'numpy.distutils.system_info.atlas_3_10_threads_info'> NOT AVAILABLE atlas_3_10_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries satlas,satlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries satlas,satlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries satlas,satlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\Library\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries satlas,satlas not found in E:\Anaconda3\Library\lib <class 'numpy.distutils.system_info.atlas_3_10_info'> NOT AVAILABLE atlas_threads_info: Setting PTATLAS=ATLAS No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries ptf77blas,ptcblas,atlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries ptf77blas,ptcblas,atlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries ptf77blas,ptcblas,atlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\Library\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries ptf77blas,ptcblas,atlas not found in E:\Anaconda3\Library\lib <class 'numpy.distutils.system_info.atlas_threads_info'> NOT AVAILABLE atlas_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries f77blas,cblas,atlas not found in E:\Anaconda3\envs\yanzhan\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries f77blas,cblas,atlas not found in C:\ No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries f77blas,cblas,atlas not found in E:\Anaconda3\envs\yanzhan\libs No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack_atlas not found in E:\Anaconda3\Library\lib No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries f77blas,cblas,atlas not found in E:\Anaconda3\Library\lib <class 'numpy.distutils.system_info.atlas_info'> NOT AVAILABLE lapack_info: No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils customize MSVCCompiler libraries lapack not found in ['E:\\Anaconda3\\envs\\yanzhan\\lib', 'C:\\', 'E:\\Anaconda3\\envs\\yanzhan\\libs', 'E:\\Anaconda3\\Library\\lib'] NOT AVAILABLE C:\Users\Administrator\AppData\Local\Temp\pip-install-hgll2ytb\numpy_4b57488625214bf0a91e82d3a36ac7de\numpy\distutils\system_info.py:1712: UserWarning: Lapack (http://www.netlib.org/lapack/) libraries not found. Directories to search for the libraries can be specified in the numpy/distutils/site.cfg file (section [lapack]) or by setting the LAPACK environment variable. if getattr(self, '_calc_info_{}'.format(lapack))(): lapack_src_info: NOT AVAILABLE C:\Users\Administrator\AppData\Local\Temp\pip-install-hgll2ytb\numpy_4b57488625214bf0a91e82d3a36ac7de\numpy\distutils\system_info.py:1712: UserWarning: Lapack (http://www.netlib.org/lapack/) sources not found. Directories to search for the sources can be specified in the numpy/distutils/site.cfg file (section [lapack_src]) or by setting the LAPACK_SRC environment variable. if getattr(self, '_calc_info_{}'.format(lapack))(): NOT AVAILABLE E:\Anaconda3\envs\yanzhan\lib\site-packages\setuptools\_distutils\dist.py:275: UserWarning: Unknown distribution option: 'define_macros' warnings.warn(msg) running bdist_wheel running build running config_cc unifing config_cc, config, build_clib, build_ext, build commands --compiler options running config_fc unifing config_fc, config, build_clib, build_ext, build commands --fcompiler options running build_src build_src building py_modules sources creating build creating build\src.win32-3.9 creating build\src.win32-3.9\numpy creating build\src.win32-3.9\numpy\distutils building library "npymath" sources No module named 'numpy.distutils._msvccompiler' in numpy.distutils; trying from distutils error: Microsoft Visual C++ 14.0 or greater is required. Get it with "Microsoft C++ Build Tools": https://visualstudio.microsoft.com/visual-cpp-build-tools/ [end of output] note: This error originates from a subprocess, and is likely not a problem with pip. ERROR: Failed building wheel for numpy Running setup.py clean for numpy error: subprocess-exited-with-error python setup.py clean did not run successfully. exit code: 1 [10 lines of output] Running from numpy source directory. `setup.py clean` is not supported, use one of the following instead: - `git clean -xdf` (cleans all files) - `git clean -Xdf` (cleans all versioned files, doesn't touch files that aren't checked into the git repo) Add `--force` to your command to use it anyway if you must (unsupported). [end of output] note: This error originates from a subprocess, and is likely not a problem with pip. ERROR: Failed cleaning build dir for numpy Failed to build numpy error: failed-wheel-build-for-install Failed to build installable wheels for some pyproject.toml based projects numpy [end of output] note: This error originates from a subprocess, and is likely not a problem with pip. error: subprocess-exited-with-error × pip subprocess to install build dependencies did not run successfully. │ exit code: 1 ╰─> See above for output. note: This error originates from a subprocess, and is likely not a problem with pip. 又报错了
09-14
#include<iostream> #include<cmath> #include <stdexcept> #include <iostream> #include <vector> #include <cmath> #include <ros/ros.h> #include <sensor_msgs/PointCloud2.h> #include <pcl_conversions/pcl_conversions.h> #include "std_msgs/Float64.h" #include "geometry_msgs/Point.h" #include "sensor_msgs/Imu.h" #include "Spline.h" #include "ins/ASENSING.h" #include <std_msgs/Bool.h> #include <visualization_msgs/Marker.h> #include "jsk_recognition_msgs/BoundingBoxArray.h" #define D_ROAD_W 0.08 //道路宽度采样间隔 [m] using namespace std; int load_go=0; // bool start_go=true;//跑包注释 bool start_go; bool load_yaw=false; bool load_location=false; bool correct_data_get=false; bool point_cloud_get = false; double imu_speed; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); double x_array[26];///改点注释1111111111111111111111111111111 double y_array[26];///改点注释2222222222222222222222 class Point { public: double x ; // m double y ; // m }; class Vehicle_State { public: double x; // m double y; // m double yaw; // degree }; class Vehicle_Trajectory { public: vector<double> x; vector<double> y; double cf; }; int last_tem_index =9; ros::Publisher pub; ros::Publisher pub_stop; /**************/ ros::Publisher marker_pub; ros::Publisher marker_all; ros::Publisher marker_all2; ros::Publisher pub_brake; //2023.8.11 /****************/ std_msgs::Float64 stop_sign; std_msgs::Float64 break_flag; //2023.8.11 Vehicle_State state; double correct_angle; double correct_x; double correct_y; double angleDegrees = 0; Point center1;//第一次更新后的左圆中心 Point center2;//第一次更新后的右圆中心 vector<Point> state_all; vector<Point> obstruct; vector<Point> obstruct_left; vector<Point> obstruct_right; vector<Point> center_left; vector<Point> center_right; vector<Point> obstruct_left_nei; vector<Point> obstruct_left_wai; vector<Point> obstruct_right_nei; vector<Point> obstruct_right_wai; vector<Point> obstruct_left_nei_new; vector<Point> obstruct_left_wai_new; vector<Point> obstruct_right_nei_new; vector<Point> obstruct_right_wai_new; vector<Point> path_global_location; Vehicle_Trajectory tra_best; int location_index = 0; int load = 0; Point center_left_final; Point center_right_final; vector<Point> left_center_sum; vector<Point> right_center_sum; double way_x[26];//改点注释141414141414141414 double way_y[26]; double distance(double x1,double y1,double x2,double y2) { return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); } Point findCircleCenter(const std::vector<Point>& points, double radius) { int n = points.size(); // Create matrices for the linear system Eigen::MatrixXd A(n, 3); Eigen::VectorXd b(n); for (int i = 0; i < n; ++i) { A(i, 0) = 2 * (points[i].x - points[0].x); A(i, 1) = 2 * (points[i].y - points[0].y); A(i, 2) = -1; b(i) = pow(points[i].x - points[0].x, 2) + pow(points[i].y - points[0].y, 2) - pow(radius, 2); } // Solve the least squares problem A * [cx, cy, d] = b Eigen::VectorXd solution = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); Point center; // Extract circle center coordinates center.x = solution(0) + points[0].x; center.y = solution(1) + points[0].y; return center; } void fitCircle(const std::vector<Point>& points, double& centerX, double& centerY, double& radius) { int n = points.size(); if (n < 3) { std::cerr << "At least 3 points are required to fit a circle." << std::endl; return; } double sumX = 0, sumY = 0, sumX2 = 0, sumY2 = 0, sumXY = 0; double sumX3 = 0, sumY3 = 0, sumX1Y2 = 0, sumX2Y1 = 0; for (const auto& p : points) { double x = p.x; double y = p.y; double x2 = x * x; double y2 = y * y; sumX += x; sumY += y; sumX2 += x2; sumY2 += y2; sumXY += x * y; sumX3 += x2 * x; sumY3 += y2 * y; sumX1Y2 += x * y2; sumX2Y1 += x2 * y; } double C = n * sumX2 - sumX * sumX; double D = n * sumXY - sumX * sumY; double E = n * sumX3 + n * sumX1Y2 - (sumX2 + sumY2) * sumX; double G = n * sumY2 - sumY * sumY; double H = n * sumY3 + n * sumX2Y1 - (sumX2 + sumY2) * sumY; double denominator = 2 * (C * G - D * D); if (denominator == 0) { std::cerr << "Denominator in circle fitting is zero." << std::endl; return; } centerX = (G * E - D * H) / denominator; centerY = (C * H - D * E) / denominator; radius = std::sqrt((sumX2 + sumY2 - 2 * centerX * sumX - 2 * centerY * sumY + n * (centerX * centerX + centerY * centerY)) / n); } void interpolatePoints(double* original_x, double* original_y, double* new_x, double* new_y,int SIZE ,int INTERVALS) { int index = 0; for (int i = 0; i < SIZE - 1; ++i) { double x1 = original_x[i]; double y1 = original_y[i]; double x2 = original_x[i + 1]; double y2 = original_y[i + 1]; double dx = (x2 - x1) / INTERVALS; double dy = (y2 - y1) / INTERVALS; for (int j = 0; j < INTERVALS; ++j) { new_x[index] = x1 + j * dx; new_y[index] = y1 + j * dy; index++; } } // Add the last point from original arrays new_x[index] = original_x[SIZE - 1]; new_y[index] = original_y[SIZE - 1]; } void set_path_global_location() { //起点长12 yuanshi double relative_x_chushi[190] = {1.5,3.5, 4.50000000000000, 6, 7.50000000000000, 9,\ 10.5000000000000, 12, 13.5000000000000, 15, 16.4929784907258, 17.9458293799335, 19.3198327455343,\ 20.5757278190614, 21.6828360254628, 22.6094864377429, 23.3294552745182, 23.8273423696585, 24.0851743056398,\ 24.0979134140882, 23.8680591804246, 23.3954329981049, 22.6979300314371, 21.7946808320437, 20.7048142903275,\ 19.4630603273459, 18.1011332458409, 16.6546373468543, 15.1641312227858, 13.6690456433255, 12.2101604570310,\ 10.8256011255841, 9.55468897394665, 8.43058413047132, 7.48147650023618, 6.73850387747837, 6.21667689392197,\ 5.92972783702215, 5.89151242648978, 6.09614442454558, 6.54058093591135, 7.21546395009521, 8.09787282472529,\ 9.16744952227656, 10.3947886499797, 11.7450087033329, 13.1840940648742, 14.6717990386110, 15, 16.4929784907258,\ 17.9458293799335, 19.3198327455343, 20.5757278190614, 21.6828360254628, 22.6094864377429, 23.3294552745182,\ 23.8273423696585, 24.0851743056398, 24.0979134140882, 23.8680591804246, 23.3954329981049, 22.6979300314371,\ 21.7946808320437, 20.7048142903275, 19.4630603273459, 18.1011332458409, 16.6546373468543, 15.1641312227858,\ 13.6690456433255, 12.2101604570310, 10.8256011255841, 9.55468897394665, 8.43058413047132, 7.48147650023618,\ 6.73850387747837, 6.21667689392197, 5.92972783702215, 5.89151242648978, 6.09614442454558, 6.54058093591135,\ 7.21546395009521, 8.09787282472529, 9.16744952227656, 10.3947886499797, 11.7450087033329, 13.1840940648742,\ 14.6717990386110, 15, 16.4929784907258, 17.9458293799335, 19.3198327455343, 20.5757278190614, 21.6828360254628,\ 22.6094864377429, 23.3294552745182, 23.8273423696585, 24.0851743056398, 24.0979134140882, 23.8680591804246,\ 23.3954329981049, 22.6979300314371, 21.7946808320437, 20.7048142903275, 19.4630603273459, 18.1011332458409,\ 16.6546373468543, 15.1641312227858, 13.6690456433255, 12.2101604570310, 10.8256011255841, 9.55468897394665,\ 8.43058413047132, 7.48147650023618, 6.73850387747837, 6.21667689392197, 5.92972783702215, 5.89151242648978,\ 6.09614442454558, 6.54058093591135, 7.21546395009521, 8.09787282472529, 9.16744952227656, 10.3947886499797,\ 11.7450087033329, 13.1840940648742, 14.6717990386110, 15, 16.4929784907258, 17.9458293799335, 19.3198327455343,\ 20.5757278190614, 21.6828360254628, 22.6094864377429, 23.3294552745182, 23.8273423696585, 24.0851743056398,\ 24.0979134140882, 23.8680591804246, 23.3954329981049, 22.6979300314371, 21.7946808320437, 20.7048142903275,\ 19.4630603273459, 18.1011332458409, 16.6546373468543, 15.1641312227858, 13.6690456433255, 12.2101604570310,\ 10.8256011255841, 9.55468897394665, 8.43058413047132, 7.48147650023618, 6.73850387747837, 6.21667689392197,\ 5.92972783702215, 5.89151242648978, 6.09614442454558, 6.54058093591135, 7.21546395009521, 8.09787282472529,\ 9.16744952227656, 10.3947886499797, 11.7450087033329, 13.1840940648742, 14.6717990386110, 15, 16.5000000000000,\ 19, 22, 25, 26, 27, 27.5, 28, 30, 32, 34,36,37,38,39,40,41,42,43,44}; double relative_y_chushi[190] = {0,0, 0, 0, 0, 0, 0, 0, 0, 0, -0.125512998485473, -0.490926224144828,\ -1.08816183366577, -1.90503550632258, -2.91432374260530, -4.09153170739197, -5.40532980676421, -6.81832568457704,\ -8.29414167929603, -9.79219795955772, -11.2726581312459, -12.6943584686520, -14.0201743080130, -15.2155116851562,\ -16.2435583500563, -17.0816257944470, -17.7058883021754, -18.0959745771550, -18.2468458811889, -18.1508600590876,\ -17.8102001737293, -17.2379453204251, -16.4446558358059, -15.4543479571398, -14.2951097755576, -12.9940963464869,\ -11.5898196518438, -10.1193407357934, -8.62161504175242, -7.13755102147194, -5.70681560192370, -4.36927451073298,\ -3.15859392661961, -2.10954692453109, -1.25047108785953, -0.601302290877508, -0.184672791857001, -0.00847771368893407,\ 0, -0.125512998485473, -0.490926224144828, -1.08816183366577, -1.90503550632258, -2.91432374260530, -4.09153170739197,\ -5.40532980676421, -6.81832568457704, -8.29414167929603, -9.79219795955772, -11.2726581312459, -12.6943584686520,\ -14.0201743080130, -15.2155116851562, -16.2435583500563, -17.0816257944470, -17.7058883021754, -18.0959745771550,\ -18.2468458811889, -18.1508600590876, -17.8102001737293, -17.2379453204251, -16.4446558358059, -15.4543479571398,\ -14.2951097755576, -12.9940963464869, -11.5898196518438, -10.1193407357934, -8.62161504175242, -7.13755102147194,\ -5.70681560192370, -4.36927451073298, -3.15859392661961, -2.10954692453109, -1.25047108785953, -0.601302290877508,\ -0.184672791857001, -0.00847771368893407, 0, 0.125512998485473, 0.490926224144828, 1.08816183366577, 1.90503550632258,\ 2.91432374260530, 4.09153170739197, 5.40532980676421, 6.81832568457704, 8.29414167929603, 9.79219795955772, 11.2726581312459,\ 12.6943584686520, 14.0201743080130, 15.2155116851562, 16.2435583500563, 17.0816257944470, 17.7058883021754, 18.0959745771550,\ 18.2468458811889, 18.1508600590876, 17.8102001737293, 17.2379453204251, 16.4446558358059, 15.4543479571398, 14.2951097755576,\ 12.9940963464869, 11.5898196518438, 10.1193407357934, 8.62161504175242, 7.13755102147194, 5.70681560192370, 4.36927451073298,\ 3.15859392661961, 2.10954692453109, 1.25047108785953, 0.601302290877508, 0.184672791857001, 0.00847771368893407, 0,\ 0.125512998485473, 0.490926224144828, 1.08816183366577, 1.90503550632258, 2.91432374260530, 4.09153170739197, 5.40532980676421,\ 6.81832568457704, 8.29414167929603, 9.79219795955772, 11.2726581312459, 12.6943584686520, 14.0201743080130, 15.2155116851562,\ 16.2435583500563, 17.0816257944470, 17.7058883021754, 18.0959745771550, 18.2468458811889, 18.1508600590876, 17.8102001737293,\ 17.2379453204251, 16.4446558358059, 15.4543479571398, 14.2951097755576, 12.9940963464869, 11.5898196518438, 10.1193407357934,\ 8.62161504175242, 7.13755102147194, 5.70681560192370, 4.36927451073298, 3.15859392661961, 2.10954692453109, 1.25047108785953,\ 0.601302290877508, 0.184672791857001, 0.00847771368893407, 0, 0, 0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; double relative_x[950]; double relative_y[950]; interpolatePoints(relative_x_chushi, relative_y_chushi, relative_x, relative_y,190,5); for(int i=0;i<950;i++) //12米 { relative_x[i]=relative_x[i]+ correct_x; relative_y[i]=relative_y[i]+ correct_y; } correct_angle = angleDegrees; double point_center_x = 15.0; double point_center_y = 0.0; //预设点集的八字中心点坐标 double cosAngle = cos(-correct_angle* M_PI/180.0); double sinAngle = sin(-correct_angle* M_PI/180.0); for(int i=0;i<950;i++) //12米 { double temp_x = relative_x[i]; double temp_y = relative_y[i]; relative_x[i] = cosAngle * (temp_x - point_center_x) - sinAngle * (temp_y - point_center_y) + point_center_x; relative_y[i] = sinAngle * (temp_x - point_center_x) + cosAngle * (temp_y - point_center_y) + point_center_y; Point temp; temp.x = relative_x[i]; temp.y = relative_y[i]; state_all.push_back(temp); } Point trans_point; for(int i=0;i<950;i++)//转换为世界坐标 改长注释22222222222222222222222222222 { trans_point.x = state.x + (relative_x[i]+2.38)*cos(state.yaw*M_PI/180) + relative_y[i]*sin(state.yaw*M_PI/180); trans_point.y = state.y - relative_y[i]*cos(state.yaw*M_PI/180) + (relative_x[i]+2.38)*sin(state.yaw*M_PI/180); path_global_location.push_back(trans_point); } } Point trans_local_xy(Point global_xy)//把相对于世界坐标的点转到激光雷达坐标系下 { Point xy_local; double trans_x = (global_xy.x-state.x)*cos(-state.yaw*M_PI/180) - (global_xy.y-state.y)*sin(-state.yaw*M_PI/180) - 2.38; double trans_y = - (global_xy.y-state.y)*cos(-state.yaw*M_PI/180) - (global_xy.x-state.x)*sin(-state.yaw*M_PI/180); xy_local.x = trans_x; xy_local.y = trans_y; return xy_local; } void start_sign(const std_msgs::Float64& msg) { load_go = msg.data; cout<<"load_go= %d!!!"<<load_go<<endl; if(load_go==1) { cout<<"start go!!!"<<endl; start_go=true; }else{ cout<<"wait for go"<<endl; } } void zhuitongCallback(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg) { obstruct.clear(); obstruct_left.clear(); obstruct_right.clear(); obstruct_left_nei.clear(); obstruct_left_wai.clear(); obstruct_right_nei.clear(); obstruct_right_wai.clear(); obstruct_left_nei_new.clear(); obstruct_left_wai_new.clear(); obstruct_right_nei_new.clear(); obstruct_right_wai_new.clear(); Point temp; for(int i = 0;i<msg->boxes.size();i++) { temp.x= msg->boxes[i].pose.position.x; temp.y= msg->boxes[i].pose.position.y; float dis = distance(0,0,temp.x,temp.y); if(dis<1.5||dis>29) { continue; } obstruct.push_back(temp); if(temp.y>0) { obstruct_left.push_back(temp); }else if(temp.y<0) { obstruct_right.push_back(temp); } } if(correct_data_get == false) { if(obstruct.size()>0) { point_cloud_get = true; } if(obstruct_left.size()>2) { Point test; test = findCircleCenter(obstruct_left,9.125); for(auto &obs_temp : obstruct_left) { double dis_temp = distance(obs_temp.x,obs_temp.y,test.x,test.y); if(dis_temp>=9.125) { obstruct_left_wai.push_back(obs_temp); }else { obstruct_left_nei.push_back(obs_temp); } } } // cout<<"x_left:"<<test.x<<endl; // cout<<"y_left:"<<test.y<<endl; if(obstruct_right.size()>2) { Point test2; test2 = findCircleCenter(obstruct_right,9.125); for(auto &obs_temp2 : obstruct_right) { double dis_temp2 = distance(obs_temp2.x,obs_temp2.y,test2.x,test2.y); if(dis_temp2>=9.125) { obstruct_right_wai.push_back(obs_temp2); }else { obstruct_right_nei.push_back(obs_temp2); } } } // cout<<"x_right:"<<test2.x<<endl; // cout<<"y_right:"<<test2.y<<endl; if(obstruct_left_nei.size()>2) { temp = findCircleCenter(obstruct_left_nei,7.625); left_center_sum.push_back(temp); } if(obstruct_left_wai.size()>2) { temp = findCircleCenter(obstruct_left_wai,10.625); left_center_sum.push_back(temp); } if(obstruct_right_nei.size()>2) { temp = findCircleCenter(obstruct_right_nei,7.625); right_center_sum.push_back(temp); } if(obstruct_right_wai.size()>2) { temp = findCircleCenter(obstruct_right_wai,10.625); right_center_sum.push_back(temp); } // cout<<"left:"<<left_center_sum.size()<<endl; // cout<<"right:"<<right_center_sum.size()<<endl; if(right_center_sum.size()>20&&left_center_sum.size()>20) { double left_x_sum = 0; double left_y_sum = 0; for(int i = 0;i<left_center_sum.size();i++) { left_x_sum +=left_center_sum[i].x; left_y_sum +=left_center_sum[i].y; } center1.x = left_x_sum/(left_center_sum.size()); center1.y = left_y_sum/(left_center_sum.size()); double right_x_sum = 0; double right_y_sum = 0; for(int i = 0;i<right_center_sum.size();i++) { right_x_sum +=right_center_sum[i].x; right_y_sum +=right_center_sum[i].y; } center2.x = right_x_sum/(right_center_sum.size()); center2.y = right_y_sum/(right_center_sum.size()); } if(center1.x!=0&&center1.y!=0&&center2.x!=0&&center2.y!=0&&obstruct_left.size()>2&&obstruct_right.size()>2) { for(auto &obs_temp : obstruct_left) { double dis_temp = distance(obs_temp.x,obs_temp.y,center1.x,center1.y); if(dis_temp>=9.125) { obstruct_left_wai_new.push_back(obs_temp); }else { obstruct_left_nei_new.push_back(obs_temp); } } for(auto &obs_temp2 : obstruct_right) { double dis_temp2 = distance(obs_temp2.x,obs_temp2.y,center2.x,center2.y); if(dis_temp2>=9.125) { obstruct_right_wai_new.push_back(obs_temp2); }else { obstruct_right_nei_new.push_back(obs_temp2); } } Point center; if(obstruct_left_nei_new.size()>2) { center = findCircleCenter(obstruct_left_nei_new,7.625); center_left.push_back(center); } if(obstruct_left_wai_new.size()>2) { center = findCircleCenter(obstruct_left_wai_new,10.625); center_left.push_back(center); } if(obstruct_right_nei_new.size()>2) { center = findCircleCenter(obstruct_right_nei_new,7.625); center_right.push_back(center); } if(obstruct_right_wai_new.size()>2) { center = findCircleCenter(obstruct_right_wai_new,10.625); center_right.push_back(center); } Point left_sum; Point right_sum; if(center_left.size()>20&&center_right.size()>20) { for(int i = 0;i<center_left.size();i++) { left_sum.x += center_left[i].x; left_sum.y += center_left[i].y; right_sum.x += center_right[i].x; right_sum.y += center_right[i].y; } center_left_final.x = left_sum.x / center_left.size(); center_left_final.y = left_sum.y / center_left.size(); center_right_final.x = right_sum.x / center_right.size(); center_right_final.y = right_sum.y / center_right.size(); } if(center_left_final.x!=0&&center_left_final.y!=0&&center_right_final.x!=0&&center_right_final.y!=0) { center_left_final.x = center_left_final.x ; center_left_final.y = center_left_final.y ; center_right_final.x = center_right_final.x ; center_right_final.y = center_right_final.y ; correct_x = (center_left_final.x + center_right_final.x)/2 - 15; correct_y = (center_left_final.y + center_right_final.y)/2 - 0; double angle = atan2(center_right_final.x - center_left_final.x, center_right_final.y - center_left_final.y); angleDegrees = angle * 180.0 / M_PI-180; if(angleDegrees<-180) { angleDegrees = angleDegrees+360.0; } correct_data_get = true; // cout<<"x:"<<correct_x+15<<endl; // cout<<"y:"<<correct_y<<endl; } } } // visualization_msgs::Marker line_strip_all; // line_strip_all.header.frame_id = "hesai40"; // line_strip_all.header.stamp = ros::Time::now(); // line_strip_all.ns = "trajectory_all"; // line_strip_all.action = visualization_msgs::Marker::ADD; // line_strip_all.pose.orientation.w = 0.0; // line_strip_all.id = 0; // line_strip_all.type = visualization_msgs::Marker::POINTS; // // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width // line_strip_all.scale.x = 0.5; // // Line strip 是绿色 // line_strip_all.color.g = 1.0f; // line_strip_all.color.a = 1.0; // for (int i = 0; i < obstruct_left_nei_new.size(); ++i) // { // geometry_msgs::Point p2; // p2.x = obstruct_left_nei_new[i].x; // p2.y = obstruct_left_nei_new[i].y;//(int32_t) // p2.z = 0; // line_strip_all.points.push_back(p2); // } // //发布各个markers // marker_all.publish(line_strip_all); visualization_msgs::Marker line_strip_all2; line_strip_all2.header.frame_id = "hesai40"; line_strip_all2.header.stamp = ros::Time::now(); line_strip_all2.ns = "trajectory_all2"; line_strip_all2.action = visualization_msgs::Marker::ADD; line_strip_all2.pose.orientation.w = 0.0; line_strip_all2.id = 0; line_strip_all2.type = visualization_msgs::Marker::POINTS; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip_all2.scale.x = 0.5; // Line strip 是绿色 line_strip_all2.color.r = 1.0f; line_strip_all2.color.g = 0; line_strip_all2.color.a = 1.0; geometry_msgs::Point p3; p3.x = center_left_final.x; p3.y = center_left_final.y;//(int32_t) p3.z = 0; line_strip_all2.points.push_back(p3); p3.x = center_right_final.x; p3.y = center_right_final.y;//(int32_t) p3.z = 0; line_strip_all2.points.push_back(p3); //发布各个markers marker_all2.publish(line_strip_all2); } void update_yaw_location(const ins::ASENSING::ConstPtr& msg) { state.yaw = msg->azimuth; load_yaw = true; state.x = msg->location_x; state.y = msg->location_y; load_location = true; imu_speed=msg->absolute_velocity; } vector<Vehicle_Trajectory> calc_frenet_paths(double x_array[],double y_array[],int array_size) { vector<Vehicle_Trajectory> frenet_paths; vector<double> x_a,y_a; vector<double> angle; vector<Point> sp; for(int i = 0;i<array_size;++i) { double at_angle; if(x_array[i] == 0) { x_array[i] = 0.000001; } at_angle = -atan(y_array[i]/x_array[i]); angle.push_back(at_angle); } for(int i=-9;i<=+9;++i) { Vehicle_Trajectory fp; vector<double> s_x; vector<double> s_y; s_x.push_back(0.0); s_y.push_back(0.0); for(int j = 0;j<array_size;++j) { Point sp_temp; sp_temp.x = x_array[j]+i*D_ROAD_W*sin(angle[j]); sp_temp.y = y_array[j]+i*D_ROAD_W*cos(angle[j]); s_x.push_back(sp_temp.x); s_y.push_back(sp_temp.y); } for(int i=0;i<10;++i) { double h_x = (s_x[i+1]-s_x[i])/9; double h_y = (s_y[i+1]-s_y[i])/9; for(int j=0;j<10;j++) { fp.x.push_back(h_x*j+s_x[i]); fp.y.push_back(h_y*j+s_y[i]); x_a.push_back(h_x*j+s_x[i]); y_a.push_back(h_y*j+s_y[i]); } } frenet_paths.push_back(fp); } return frenet_paths; } Vehicle_Trajectory calc_cost(Vehicle_Trajectory tr_p,vector<Point> ob)///计算路径点中离锥桶的最小距离 { tr_p.cf = 100; for(int i=0;i<ob.size();i++) { for(int j=0;j<tr_p.x.size();j++) { double dist = distance(ob[i].x, ob[i].y,tr_p.x[j], tr_p.y[j]); if(dist<tr_p.cf) { tr_p.cf = dist; } } } return tr_p; } vector<Vehicle_Trajectory> check_paths(vector<Vehicle_Trajectory> fplist, vector<Point> ob) { vector<Vehicle_Trajectory> fpl; Vehicle_Trajectory calc; cout<<"ob.size()="<<ob.size()<<endl; for(int i=0;i<fplist.size();i++) { calc = calc_cost(fplist[i],ob);// 碰撞检查 //计算每条路径点中离锥桶的最小距离 fpl.push_back(calc); } return fpl; } Vehicle_Trajectory frenet_optimal_planning_nopoint(double x_array[],double y_array[],int array_size) { vector<Vehicle_Trajectory> fplist; fplist = calc_frenet_paths(x_array,y_array,array_size); Vehicle_Trajectory bestpath = fplist[last_tem_index]; return bestpath; } Vehicle_Trajectory frenet_optimal_planning(vector<Point> ob,double x_array[],double y_array[],int &last_tem_index,int array_size) //最佳路径计算函数 { vector<Vehicle_Trajectory> fplist; fplist = calc_frenet_paths(x_array,y_array,array_size); //cout<<"fplist.size()="<<fplist.size()<<endl; vector<Vehicle_Trajectory> fpl_1; fpl_1 = check_paths(fplist, ob);/////////计算每条路径与锥桶的最小距离 // cout<<"fpl_1.size()="<<fpl_1.size()<<endl; int tem_index =last_tem_index; int size=fpl_1.size(); if(location_index<10) { size=fpl_1.size()/2; } int start=0; if(location_index>84&&location_index<100) { start=0; size=fpl_1.size()*1/2; } if(location_index>164&&location_index<180) { start=fpl_1.size()/2; size=fpl_1.size()*2/3; cout<<"!!!!!!!!!!!!!!!!!!!!!!!"<<endl; cout<<"33333333333333333333333"<<endl; } if(location_index>=0&&location_index<10) { for(start;start<size;start++) { if(abs(start-last_tem_index)<3) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { tem_index = start; } } } } if(location_index>=10&&location_index<84) { for(start;start<size;start++) { if(abs(start-last_tem_index)<3) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { for(start;start<16;start++) { double t=fpl_1[start].cf; if(t>1.0) { tem_index = start; break;} } if(tem_index>13){tem_index=13;} if(tem_index<4){tem_index=4;} } } } } if(location_index>=84&&location_index<90) { for(start;start<size;start++) { if(abs(start-last_tem_index)<4) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { double t=fpl_1[start].cf; if(t>1.0) { tem_index = start; if(tem_index>13){tem_index=13;} if(tem_index<4){tem_index=4;} break;} } } } } if(location_index>=90&&location_index<180) { for(start;start<size;start++) { if(abs(start-last_tem_index)<4) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { double t=fpl_1[start].cf; if(t>1.0&&t<2.0) { tem_index = start; if(tem_index>13){tem_index=13;} if(tem_index<4){tem_index=4;} break;} } } } } cout<<"tem_index="<<tem_index<<endl; Vehicle_Trajectory bestpath = fpl_1[tem_index]; last_tem_index =tem_index; return bestpath; } int search_index(Vehicle_State point_search,vector<Point> point_all) { Vehicle_State current_point; Vehicle_State point; std::map<double,Vehicle_State>min_distance; for(auto &iter_p : point_all) { double distance2 = std::sqrt((point_search.x - iter_p.x)*(point_search.x - iter_p.x)+(point_search.y - iter_p.y)*(point_search.y - iter_p.y)); current_point.x=iter_p.x; current_point.y=iter_p.y; min_distance.insert(std::make_pair(distance2,current_point)); } auto min = min_distance.begin(); point = min->second; for(int i=0;i<point_all.size();i++) { if(point.x==point_all[i].x&&point.y==point_all[i].y) { if(point.x<point_search.x) { return i+1; } return i; } } return 0; }; void path_plan() { if(load_yaw ==true && load_location==true && correct_data_get==true) { sensor_msgs::PointCloud2 output; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>); if(load == 0) { set_path_global_location(); load++; location_index = search_index(state,path_global_location); cout<<"location_index:"<<location_index<<endl; if(location_index>50) { location_index = 50; } location_index = 0; } double distance;//车前路径点与车的距离 distance = hypot((path_global_location[location_index].x - state.x),(path_global_location[location_index].y - state.y)); ROS_INFO("distance=%f",distance); if(distance <4)//1111111111111111111111111111111111111111111111111小心!!!!!!!!!!!!!! { for(int i=0;i<26;i++)//车前路径点和顺下点 ///改点注释3333333333333333333333333333333333333 { way_x[i] = path_global_location[location_index+i].x; way_y[i] = path_global_location[location_index+i].y; cout<<"x:"<<way_x[i]<<setprecision(8)<<endl; cout<<"y:"<<way_y[i]<<setprecision(8)<<endl; } location_index++; } break_flag.data=0; //2023.8.11 if(location_index==435)//87 { break_flag.data=35; std::cout<<break_flag.data<<"yyyyyyyyyyyyyyyyyyyyy"<<std::endl; } if(location_index==430)//86 { break_flag.data=45; std::cout<<break_flag.data<<"yyyyyyyyyyyyyyyyyyyyy"<<std::endl; } pub_brake.publish(break_flag); //2023.8.11 for(int i=0;i<26;i++)//将路径点转换为激光雷达坐标下的点///改点注释444444444444444444444444444444 { Point tr_xy; Point lo_xy; tr_xy.x = way_x[i]; tr_xy.y = way_y[i]; lo_xy = trans_local_xy(tr_xy); x_array[i] = lo_xy.x; y_array[i] = lo_xy.y; } int array_size = sizeof(x_array)/sizeof(x_array[0]); if(point_cloud_get==true) { tra_best = frenet_optimal_planning(obstruct,x_array,y_array,last_tem_index,array_size); point_cloud_get = false; } else { tra_best = frenet_optimal_planning_nopoint(x_array,y_array,array_size); } if(location_index >= 850) { stop_sign.data = 1; cout<<" stop_sign.data = "<<stop_sign.data<<endl; ROS_INFO("Vehicle has reached the target point! Node stop!"); pub_stop.publish(stop_sign); } (*cloud1).width=tra_best.x.size();//zuiyou lujing //无序点云width为点云大小,heigh为1 (*cloud1).height=1; (*cloud1).is_dense=false; //默认为false (*cloud1).points.resize(tra_best.x.size()); //点云大小 for(int i=0;i<tra_best.x.size();i++) { (*cloud1)[i].x=tra_best.x[i]; (*cloud1)[i].y=tra_best.y[i]; // cout<<"(*cloud1)[i].x="<<(*cloud1)[i].x<<endl; // cout<<"(*cloud1)[i].y="<<(*cloud1)[i].y<<endl; (*cloud1)[i].z=0; (*cloud1)[i].r=0; (*cloud1)[i].g=255; (*cloud1)[i].b=0; } pcl::toROSMsg(*cloud1,output); //第一个参数是输入,后面的是输出 output.header.frame_id = "hesai40";//输出的坐标 pub.publish(output);///修改注释11111111111111111111111111111111111111111111111111111 visualization_msgs::Marker line_strip; line_strip.header.frame_id = "hesai40"; line_strip.header.stamp = ros::Time::now(); line_strip.ns = "trajectory"; line_strip.action = visualization_msgs::Marker::ADD; line_strip.pose.orientation.w = 0.0; line_strip.id = 0; line_strip.type = visualization_msgs::Marker::LINE_STRIP; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip.scale.x = 0.1; // Line strip 是绿色 line_strip.color.g = 1.0f; line_strip.color.a = 1.0; // cout<<"size:"<<tra_best.x.size()<<endl; for (int i = 0; i < tra_best.x.size(); ++i) { geometry_msgs::Point p; p.x = tra_best.x[i]; p.y = tra_best.y[i];//(int32_t) p.z = 0; line_strip.points.push_back(p); } //发布各个markers marker_pub.publish(line_strip); visualization_msgs::Marker line_strip_all; line_strip_all.header.frame_id = "hesai40"; line_strip_all.header.stamp = ros::Time::now(); line_strip_all.ns = "trajectory_all"; line_strip_all.action = visualization_msgs::Marker::ADD; line_strip_all.pose.orientation.w = 0.0; line_strip_all.id = 0; line_strip_all.type = visualization_msgs::Marker::LINE_STRIP; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip_all.scale.x = 0.1; // Line strip 是绿色 line_strip_all.color.g = 1.0f; line_strip_all.color.a = 1.0; for (int i = 0; i < path_global_location.size(); ++i) { geometry_msgs::Point p2; Point global_point; global_point = trans_local_xy(path_global_location[i]); p2.x = global_point.x; p2.y = global_point.y;//(int32_t) p2.z = 0; line_strip_all.points.push_back(p2); } //发布各个markers marker_all.publish(line_strip_all); // visualization_msgs::Marker line_strip_all; // line_strip_all.header.frame_id = "hesai40"; // line_strip_all.header.stamp = ros::Time::now(); // line_strip_all.ns = "trajectory_all"; // line_strip_all.action = visualization_msgs::Marker::ADD; // line_strip_all.pose.orientation.w = 0.0; // line_strip_all.id = 0; // line_strip_all.type = visualization_msgs::Marker::POINTS; // // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width // line_strip_all.scale.x = 0.5; // // Line strip 是绿色 // line_strip_all.color.g = 1.0f; // line_strip_all.color.a = 1.0; // for (int i = 0; i < obstruct_left_nei.size(); ++i) // { // geometry_msgs::Point p2; // p2.x = obstruct_left_nei[i].x; // p2.y = obstruct_left_nei[i].y;//(int32_t) // p2.z = 0; // line_strip_all.points.push_back(p2); // } // //发布各个markers // marker_all.publish(line_strip_all); } } int main(int argc,char** argv) { ros::init(argc,argv,"trajectory"); ros::NodeHandle n; stop_sign.data = 0; ros::Rate r(100); ros::Subscriber sub_ecu = n.subscribe ("/ECU_Data", 1, start_sign); ros::Subscriber sub_yaw_location = n.subscribe ("/INS/ASENSING_INS", 1, update_yaw_location); ros::Subscriber pub_bbox = n.subscribe<jsk_recognition_msgs::BoundingBoxArray>("/detections", 10, zhuitongCallback); marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1); marker_all = n.advertise<visualization_msgs::Marker>("visualization_all", 1); marker_all2 = n.advertise<visualization_msgs::Marker>("visualization_all2", 1); pub = n.advertise<sensor_msgs::PointCloud2> ("/trajectory", 1);//定义一个发布者,发布最后的轨迹////修改注释222222222222 pub_stop = n.advertise<std_msgs::Float64>("/stop_sign",1); pub_brake = n.advertise<std_msgs::Float64>("/brake",1); while (ros::ok()) { path_plan(); ros::spinOnce(); // Keeps node alive basically r.sleep(); // Sleep for loop_rate } } 上述代码是一个八字绕环路径规划的代码 路径是什么样的呢?你能画出来吗
10-07
import sensor, time, image,display from pyb import UART # ------------------- 1. 场地适配常量配置 ------------------- DEBUG_MODE =True# 调试模式开关 # 摄像头配置 lcd = display.SPIDisplay() CAM_PIX_FORMAT = sensor.RGB565 CAM_FRAME_SIZE = sensor.QQVGA # 160x120,保证帧率 CAM_SKIP_FRAMES = 500 # 串口配置 UART_PORT = 3 UART_BAUDRATE = 115200 UART_DELAY_MS = 10 # 目标识别参数(适配场地目标大小) BLOB_MIN = 50 # 场地目标球较小,缩小最小像素面积 BLOB_MAX = 400 # 缩小最大像素面积,过滤干扰 ROUNDNESS_THR = 0.6 # 降低圆度阈值,适配场地目标 SAFE_MIN = 1200 # 安全区面积适配(600×300mm对应像素面积) # 麦轮小车参数 GRAB_DIST = 20 # 场地目标近,缩小抓取距离(单位:cm) DIST_COEF = 350 # 重新校准距离系数(实际距离=350÷小球像素宽度) X_THR = 6 # 缩小X轴居中阈值(麦轮精准平移) Y_THR = 6 # 缩小Y轴距离阈值 TARGET_LOSS = 6 # 缩短目标丢失判定帧数,快速响应 # 黑线边界识别参数(核心) BOUND_LINE_THRESH = (0, 25, -128, 127, -128, 127) # 黑色线阈值(需实测调试) LINE_MIN_WIDTH = 2 # 场地黑线宽1-2cm,设为2像素 LINE_MIN_LENGTH = 15 # 场地黑线长,设为15像素 NEAR_LINE_DIST = 10 # 靠近黑线判定距离(10像素) # 麦轮搜索策略(沿黑线边缘平移+旋转) SEARCH_STEPS = [ (0, 0, 25), # 1. 原地顺时针旋转(扫描周围) (25, 0, 0), # 2. 向右平移 (0, 25, 0), # 3. 向前平移 (-25, 0, 0), # 4. 向左平移 (0, -25, 0), # 5. 向后平移 ] search_idx = 0 SEARCH_DELAY = 1500 # 缩短搜索切换间隔,加快搜索效率 # 指令定义(麦轮全向移动适配) CMD_MOVE = "M" # 基础移动:M,X,Y,旋转 CMD_STOP = "S" # 停止 CMD_ADJUST = "A" # 精准微调:A,X偏差,Y偏差,距离 CMD_RETURN = "H" # 复位 CMD_CLAW_GRAB = "G"#抓取 CMD_CLAW_RELEASE = "R"#释放 # 状态反馈指令 CMD_GRAB_OK = "GRAB_OK" # 抓取成功 CMD_DELIVER_OK = "DELIVER_OK" # 投递成功 CMD_TASK_FINISH = "TASK_FINISH"#任务完成 CMD_RETRY = "RETRY" # 重试 CMD_NEAR_BOUND = "NEAR_BOUND"#靠近边界(视觉识别后标记) # ------------------- 2. 全局变量初始化 ------------------- sensor.reset() sensor.set_pixformat(CAM_PIX_FORMAT) sensor.set_framesize(CAM_FRAME_SIZE) sensor.set_vflip(True) # 垂直翻转 sensor.set_hmirror(True) # 水平镜像 sensor.skip_frames(time=CAM_SKIP_FRAMES) sensor.set_auto_gain(False) sensor.set_auto_whitebal(False) uart = UART(UART_PORT, UART_BAUDRATE, timeout_char=100) # 画面有效区域(避开出发区半圆,聚焦赛场核心) img_center = (sensor.width()//2, sensor.height()//2) SCREEN_LEFT = 20 SCREEN_RIGHT = sensor.width() - 20 SCREEN_TOP = 20 SCREEN_BOTTOM = sensor.height() - 20 # 核心状态变量 bound_color = None # 绑定安全区颜色(红/蓝) normal_queue = [] # 普通目标队列(绑定颜色的球) black_queue = [] # 黑色目标队列 is_transporting = False current_target = None is_near_bound = False target_loss = 0 last_search = time.ticks_ms() # ------------------- 3. 工具函数 ------------------- def send_msg(msg): try: uart.write(f"{msg}\n") if DEBUG_MODE: print(f"[发送] {msg}") time.sleep_ms(5) except Exception as e: if DEBUG_MODE: print(f"串口错误:{e}") def recv_msg(): if uart.any(): try: recv = uart.readline().decode().strip() if recv and DEBUG_MODE: print(f"[接收] {recv}") return recv except Exception as e: if DEBUG_MODE: print(f"接收错误:{e}") return None def detect_blob(img, thr, min_p, max_p=None): blobs = img.find_blobs([thr], pixels_threshold=min_p, area_threshold=min_p) if max_p: blobs = [b for b in blobs if b.pixels() <= max_p] return blobs def detect_black_boundary(img): """检测场地黑色边界线,返回是否靠近及方向""" global is_near_bound line_blobs = detect_blob(img, BOUND_LINE_THRESH, LINE_MIN_LENGTH) near_dir = None for b in line_blobs: if b.w() > 0 and b.h()/b.w() > 3: # 过滤线特征 if b.x() < NEAR_LINE_DIST: near_dir = "left" elif (sensor.width() - (b.x() + b.w())) < NEAR_LINE_DIST: near_dir = "right" elif b.y() < NEAR_LINE_DIST: near_dir = "top" elif (sensor.height() - (b.y() + b.h())) < NEAR_LINE_DIST: near_dir = "bottom" is_near_bound = (near_dir is not None) if DEBUG_MODE and is_near_bound: print(f"靠近黑线边界:{near_dir}") return near_dir def get_xy_adjust(coord): """计算X/Y轴微调量""" x_diff = coord[0] - img_center[0] y_diff = coord[1] - img_center[1] return x_diff, y_diff def calc_dist(blob_w): """计算实际距离""" return DIST_COEF // blob_w if blob_w > 0 else 999 def is_safe(blob, safe_area): """判断是否到达安全区""" if not safe_area or not blob: return False s = safe_area[0] return (s.x() < blob.x() and s.x()+s.w() > blob.x()+blob.w() and s.y() < blob.y() and s.y()+s.h() > blob.y()+blob.h()) def update_queue(blobs, queue, type, safe_area): """更新目标队列(过滤边界外目标)""" new_ts = [] for b in blobs: if (SCREEN_LEFT < b.cx() < SCREEN_RIGHT and SCREEN_TOP < b.cy() < SCREEN_BOTTOM and not is_safe(b, safe_area) and b.roundness() >= ROUNDNESS_THR): new_t = (type, b.cx(), b.cy(), calc_dist(b.w()), b.w()) is_dup = any(abs(new_t[1]-t[1])<8 and abs(new_t[2]-t[2])<8 and abs(new_t[4]-t[4])/t[4]<0.2 for t in queue) if not is_dup: new_ts.append(new_t) new_ts.sort(key=lambda t: t[3]) queue[:] = new_ts[:3] def search_target(near_dir): """沿黑线边缘优化搜索策略""" global search_idx, last_search if time.ticks_diff(time.ticks_ms(), last_search) > SEARCH_DELAY: if is_near_bound and near_dir is not None: # 靠近黑线时:沿边界反向平移 if near_dir == "left": search_idx = 1 # 向右平移 elif near_dir == "right": search_idx = 3 # 向左平移 elif near_dir == "top": search_idx = 4 # 向后平移 elif near_dir == "bottom": search_idx = 2 # 向前平移 else: search_idx = (search_idx + 1) % len(SEARCH_STEPS) last_search = time.ticks_ms() if not is_transporting: x_spd, y_spd, rot_spd = SEARCH_STEPS[search_idx] send_msg(f"{CMD_MOVE},{x_spd},{y_spd},{rot_spd}") def track_target(img, safe_area, near_dir): """跟踪目标(新增防越界逻辑)""" global current_target, target_loss if not current_target: return "UNKNOWN" t_type, t_cx, t_cy, t_dist, t_w = current_target thr_key = "black_ball" if t_type == "DANGER" else f"{bound_color}_ball" blobs = detect_blob(img, COLOR_THRESHOLDS[thr_key], BLOB_MIN, BLOB_MAX) for b in blobs: if (abs(b.cx()-t_cx) < 20 and abs(b.cy()-t_cy) < 20 and abs(b.w()-t_w)/t_w < 0.3): # 目标在边界外→先回安全区 if is_near_bound and (b.cx() < SCREEN_LEFT or b.cx() > SCREEN_RIGHT or b.cy() < SCREEN_TOP or b.cy() > SCREEN_BOTTOM): x_adjust = SCREEN_LEFT - b.cx() if b.cx() < SCREEN_LEFT else (SCREEN_RIGHT - b.cx()) if b.cx() > SCREEN_RIGHT else 0 y_adjust = SCREEN_TOP - b.cy() if b.cy() < SCREEN_TOP else (SCREEN_BOTTOM - b.cy()) if b.cy() > SCREEN_BOTTOM else 0 send_msg(f"{CMD_ADJUST},{x_adjust},{y_adjust},{t_dist}") target_loss = 0 return "TRACKING" if is_safe(b, safe_area): return "DELIVERED" new_dist = calc_dist(b.w()) current_target = (t_type, b.cx(), b.cy(), new_dist, b.w()) x_diff, y_diff = get_xy_adjust((b.cx(), b.cy())) if (new_dist <= GRAB_DIST and abs(x_diff) <= X_THR and abs(y_diff) <= Y_THR and SCREEN_LEFT < b.cx() < SCREEN_RIGHT and SCREEN_TOP < b.cy() < SCREEN_BOTTOM): return "AT_GRAB" send_msg(f"{CMD_ADJUST},{x_diff},{y_diff},{new_dist}") target_loss = 0 return "TRACKING" target_loss += 1 if target_loss >= TARGET_LOSS: target_loss = 0 return "LOST" return "TRACKING" # ------------------- 4. 颜色阈值配置(需实测调试!) ------------------- COLOR_THRESHOLDS = { "red_ball": (3, 30, 15, 60, 45, -25), # 红色小球(适配场地) "blue_ball": (5, 62, -43, 0, -21, -4), # 蓝色小球(适配场地) "black_ball": (0, 20, -128, 127, -128, 127),# 黑色小球 "red_safe": (25, 90, 10, 127, 10, 127), # 红色安全区 "blue_safe": (25, 90, -60, -10, -30, 30) # 蓝色安全区 } # ------------------- 5. 主循环(适配场地救援任务) ------------------- if DEBUG_MODE: print("麦轮救援机器人程序启动(黑线场地适配)") send_msg(CMD_CLAW_RELEASE) time.sleep_ms(500) clock = time.clock() try: while True: clock.tick() img = sensor.snapshot() lcd.write(sensor.snapshot()) # 步骤1:检测黑线边界 near_dir = detect_black_boundary(img) # 步骤2:检测目标与安全区 red_balls = detect_blob(img, COLOR_THRESHOLDS["red_ball"], BLOB_MIN, BLOB_MAX) blue_balls = detect_blob(img, COLOR_THRESHOLDS["blue_ball"], BLOB_MIN, BLOB_MAX) black_balls = [b for b in detect_blob(img, COLOR_THRESHOLDS["black_ball"], BLOB_MIN, BLOB_MAX) if b.roundness() >= ROUNDNESS_THR-0.1] red_safe = detect_blob(img, COLOR_THRESHOLDS["red_safe"], SAFE_MIN) blue_safe = detect_blob(img, COLOR_THRESHOLDS["blue_safe"], SAFE_MIN) # 步骤3:绑定安全区颜色(抽签确定红/蓝) if not bound_color: if red_balls: # 优先识别安全区颜色,绑定后跟踪对应颜色目标 bound_color = "red" safe_area = red_safe time.sleep_ms(2) if DEBUG_MODE: print("绑定红色安全区,跟踪红色目标") elif blue_balls: bound_color = "blue" safe_area = blue_safe time.sleep_ms(2) if DEBUG_MODE: print("绑定蓝色安全区,跟踪蓝色目标") else: search_target(near_dir) time.sleep_ms(UART_DELAY_MS) continue else: safe_area = red_safe if bound_color == "red" else blue_safe # 步骤4:更新目标队列 update_queue(red_balls if bound_color == "red" else blue_balls, normal_queue, "NORMAL", safe_area) if len(normal_queue) == 0: update_queue(black_balls, black_queue, "DANGER", safe_area) # 步骤5:运输逻辑 if not is_transporting: if normal_queue: current_target = normal_queue.pop(0) is_transporting = True t_type, cx, cy, dist, w = current_target x_diff, y_diff = get_xy_adjust((cx, cy)) send_msg(f"T,{t_type},{bound_color},{cx},{cy},{x_diff},{y_diff},{dist}") elif black_queue: current_target = black_queue.pop(0) is_transporting = True t_type, cx, cy, dist, w = current_target x_diff, y_diff = get_xy_adjust((cx, cy)) send_msg(f"T,{t_type},black,{cx},{cy},{x_diff},{y_diff},{dist}") else: all_done = (len(red_balls) == 0 and len(blue_balls) == 0 and len(black_balls) == 0) if all_done: send_msg(CMD_STOP) send_msg(CMD_TASK_FINISH) if DEBUG_MODE: print("所有目标救援完成!") time.sleep_ms(1000) else: search_target(near_dir) else: track_stat = track_target(img, safe_area, near_dir) if track_stat == "LOST": send_msg(CMD_STOP) send_msg(CMD_RETRY) if current_target[0] == "NORMAL": normal_queue.append(current_target) else: black_queue.append(current_target) is_transporting = False current_target = None if DEBUG_MODE: print("目标丢失,重试搜索") elif track_stat == "AT_GRAB": send_msg(CMD_STOP) time.sleep_ms(200) send_msg(CMD_CLAW_GRAB) send_msg(CMD_GRAB_OK) if DEBUG_MODE: print("已发送抓取指令") time.sleep_ms(500) elif track_stat == "DELIVERED": send_msg(CMD_STOP) time.sleep_ms(200) send_msg(CMD_CLAW_RELEASE) send_msg(CMD_DELIVER_OK) if DEBUG_MODE: print("已投递到安全区,释放目标") time.sleep_ms(500) is_transporting = False current_target = None # 步骤6:处理反馈 recv = recv_msg() if recv == CMD_TASK_FINISH: bound_color = None normal_queue.clear() black_queue.clear() is_transporting = False current_target = None send_msg(CMD_CLAW_RELEASE) if DEBUG_MODE: print("任务完成,重置状态") elif recv == CMD_RETRY and current_target: if current_target[0] == "NORMAL": normal_queue.append(current_target) else: black_queue.append(current_target) is_transporting = False current_target = None if DEBUG_MODE: print("接收重试指令,目标重新入队") # 调试信息 if DEBUG_MODE: print(f"帧率:{clock.fps():.1f} | 绑定区:{bound_color} | 队列:{len(normal_queue)}/{len(black_queue)} | 运输中:{is_transporting} | 靠黑线:{is_near_bound}") time.sleep_ms(UART_DELAY_MS) except Exception as e: send_msg(CMD_STOP) send_msg(CMD_CLAW_RELEASE) if DEBUG_MODE: print(f"程序异常:{e}") finally: send_msg(CMD_STOP) send_msg(CMD_CLAW_RELEASE) sensor.reset() if DEBUG_MODE: print("程序退出") 根据该openmv的代码生成arduion的代码
最新发布
11-08
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值