├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── camera_info └── camera.yaml ├── include └── win_camera │ ├── visibility_control.hpp │ ├── win_camera_node.hpp │ └── wincapture.h ├── launch ├── win_camera.launch.py └── win_camera_components.launch.py ├── msg └── MFSample.msg ├── package.xml ├── rviz └── default_view.rviz └── src ├── main.cpp ├── win_camera_node.cpp └── wincapture.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | ## Ignore Visual Studio temporary files, build results, and 2 | ## files generated by popular Visual Studio add-ons. 3 | ## 4 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 5 | 6 | # User-specific files 7 | *.suo 8 | *.user 9 | *.userosscache 10 | *.sln.docstates 11 | 12 | # User-specific files (MonoDevelop/Xamarin Studio) 13 | *.userprefs 14 | 15 | # Build results 16 | [Dd]ebug/ 17 | [Dd]ebugPublic/ 18 | [Rr]elease/ 19 | [Rr]eleases/ 20 | x64/ 21 | x86/ 22 | bld/ 23 | [Bb]in/ 24 | [Oo]bj/ 25 | [Ll]og/ 26 | 27 | # Visual Studio 2015/2017 cache/options directory 28 | .vs/ 29 | # Uncomment if you have tasks that create the project's static files in wwwroot 30 | #wwwroot/ 31 | 32 | # Visual Studio 2017 auto generated files 33 | Generated\ Files/ 34 | 35 | # MSTest test Results 36 | [Tt]est[Rr]esult*/ 37 | [Bb]uild[Ll]og.* 38 | 39 | # NUNIT 40 | *.VisualState.xml 41 | TestResult.xml 42 | 43 | # Build Results of an ATL Project 44 | [Dd]ebugPS/ 45 | [Rr]eleasePS/ 46 | dlldata.c 47 | 48 | # Benchmark Results 49 | BenchmarkDotNet.Artifacts/ 50 | 51 | # .NET Core 52 | project.lock.json 53 | project.fragment.lock.json 54 | artifacts/ 55 | **/Properties/launchSettings.json 56 | 57 | # StyleCop 58 | StyleCopReport.xml 59 | 60 | # Files built by Visual Studio 61 | *_i.c 62 | *_p.c 63 | *_i.h 64 | *.ilk 65 | *.meta 66 | *.obj 67 | *.iobj 68 | *.pch 69 | *.pdb 70 | *.ipdb 71 | *.pgc 72 | *.pgd 73 | *.rsp 74 | *.sbr 75 | *.tlb 76 | *.tli 77 | *.tlh 78 | *.tmp 79 | *.tmp_proj 80 | *.log 81 | *.vspscc 82 | *.vssscc 83 | .builds 84 | *.pidb 85 | *.svclog 86 | *.scc 87 | 88 | # Chutzpah Test files 89 | _Chutzpah* 90 | 91 | # Visual C++ cache files 92 | ipch/ 93 | *.aps 94 | *.ncb 95 | *.opendb 96 | *.opensdf 97 | *.sdf 98 | *.cachefile 99 | *.VC.db 100 | *.VC.VC.opendb 101 | 102 | # Visual Studio profiler 103 | *.psess 104 | *.vsp 105 | *.vspx 106 | *.sap 107 | 108 | # Visual Studio Trace Files 109 | *.e2e 110 | 111 | # TFS 2012 Local Workspace 112 | $tf/ 113 | 114 | # Guidance Automation Toolkit 115 | *.gpState 116 | 117 | # ReSharper is a .NET coding add-in 118 | _ReSharper*/ 119 | *.[Rr]e[Ss]harper 120 | *.DotSettings.user 121 | 122 | # JustCode is a .NET coding add-in 123 | .JustCode 124 | 125 | # TeamCity is a build add-in 126 | _TeamCity* 127 | 128 | # DotCover is a Code Coverage Tool 129 | *.dotCover 130 | 131 | # AxoCover is a Code Coverage Tool 132 | .axoCover/* 133 | !.axoCover/settings.json 134 | 135 | # Visual Studio code coverage results 136 | *.coverage 137 | *.coveragexml 138 | 139 | # NCrunch 140 | _NCrunch_* 141 | .*crunch*.local.xml 142 | nCrunchTemp_* 143 | 144 | # MightyMoose 145 | *.mm.* 146 | AutoTest.Net/ 147 | 148 | # Web workbench (sass) 149 | .sass-cache/ 150 | 151 | # Installshield output folder 152 | [Ee]xpress/ 153 | 154 | # DocProject is a documentation generator add-in 155 | DocProject/buildhelp/ 156 | DocProject/Help/*.HxT 157 | DocProject/Help/*.HxC 158 | DocProject/Help/*.hhc 159 | DocProject/Help/*.hhk 160 | DocProject/Help/*.hhp 161 | DocProject/Help/Html2 162 | DocProject/Help/html 163 | 164 | # Click-Once directory 165 | publish/ 166 | 167 | # Publish Web Output 168 | *.[Pp]ublish.xml 169 | *.azurePubxml 170 | # Note: Comment the next line if you want to checkin your web deploy settings, 171 | # but database connection strings (with potential passwords) will be unencrypted 172 | *.pubxml 173 | *.publishproj 174 | 175 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 176 | # checkin your Azure Web App publish settings, but sensitive information contained 177 | # in these scripts will be unencrypted 178 | PublishScripts/ 179 | 180 | # NuGet Packages 181 | *.nupkg 182 | # The packages folder can be ignored because of Package Restore 183 | **/[Pp]ackages/* 184 | # except build/, which is used as an MSBuild target. 185 | !**/[Pp]ackages/build/ 186 | # Uncomment if necessary however generally it will be regenerated when needed 187 | #!**/[Pp]ackages/repositories.config 188 | # NuGet v3's project.json files produces more ignorable files 189 | *.nuget.props 190 | *.nuget.targets 191 | 192 | # Microsoft Azure Build Output 193 | csx/ 194 | *.build.csdef 195 | 196 | # Microsoft Azure Emulator 197 | ecf/ 198 | rcf/ 199 | 200 | # Windows Store app package directories and files 201 | AppPackages/ 202 | BundleArtifacts/ 203 | Package.StoreAssociation.xml 204 | _pkginfo.txt 205 | *.appx 206 | 207 | # Visual Studio cache files 208 | # files ending in .cache can be ignored 209 | *.[Cc]ache 210 | # but keep track of directories ending in .cache 211 | !*.[Cc]ache/ 212 | 213 | # Others 214 | ClientBin/ 215 | ~$* 216 | *~ 217 | *.dbmdl 218 | *.dbproj.schemaview 219 | *.jfm 220 | *.pfx 221 | *.publishsettings 222 | orleans.codegen.cs 223 | 224 | # Including strong name files can present a security risk 225 | # (https://github.com/github/gitignore/pull/2483#issue-259490424) 226 | #*.snk 227 | 228 | # Since there are multiple workflows, uncomment next line to ignore bower_components 229 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 230 | #bower_components/ 231 | 232 | # RIA/Silverlight projects 233 | Generated_Code/ 234 | 235 | # Backup & report files from converting an old project file 236 | # to a newer Visual Studio version. Backup files are not needed, 237 | # because we have git ;-) 238 | _UpgradeReport_Files/ 239 | Backup*/ 240 | UpgradeLog*.XML 241 | UpgradeLog*.htm 242 | ServiceFabricBackup/ 243 | *.rptproj.bak 244 | 245 | # SQL Server files 246 | *.mdf 247 | *.ldf 248 | *.ndf 249 | 250 | # Business Intelligence projects 251 | *.rdl.data 252 | *.bim.layout 253 | *.bim_*.settings 254 | *.rptproj.rsuser 255 | 256 | # Microsoft Fakes 257 | FakesAssemblies/ 258 | 259 | # GhostDoc plugin setting file 260 | *.GhostDoc.xml 261 | 262 | # Node.js Tools for Visual Studio 263 | .ntvs_analysis.dat 264 | node_modules/ 265 | 266 | # Visual Studio 6 build log 267 | *.plg 268 | 269 | # Visual Studio 6 workspace options file 270 | *.opt 271 | 272 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 273 | *.vbw 274 | 275 | # Visual Studio LightSwitch build output 276 | **/*.HTMLClient/GeneratedArtifacts 277 | **/*.DesktopClient/GeneratedArtifacts 278 | **/*.DesktopClient/ModelManifest.xml 279 | **/*.Server/GeneratedArtifacts 280 | **/*.Server/ModelManifest.xml 281 | _Pvt_Extensions 282 | 283 | # Paket dependency manager 284 | .paket/paket.exe 285 | paket-files/ 286 | 287 | # FAKE - F# Make 288 | .fake/ 289 | 290 | # JetBrains Rider 291 | .idea/ 292 | *.sln.iml 293 | 294 | # CodeRush 295 | .cr/ 296 | 297 | # Python Tools for Visual Studio (PTVS) 298 | __pycache__/ 299 | *.pyc 300 | 301 | # Cake - Uncomment if you are using it 302 | # tools/** 303 | # !tools/packages.config 304 | 305 | # Tabs Studio 306 | *.tss 307 | 308 | # Telerik's JustMock configuration file 309 | *.jmconfig 310 | 311 | # BizTalk build output 312 | *.btp.cs 313 | *.btm.cs 314 | *.odx.cs 315 | *.xsd.cs 316 | 317 | # OpenCover UI analysis results 318 | OpenCover/ 319 | 320 | # Azure Stream Analytics local run output 321 | ASALocalRun/ 322 | 323 | # MSBuild Binary and Structured Log 324 | *.binlog 325 | 326 | # NVidia Nsight GPU debugger configuration file 327 | *.nvuser 328 | 329 | # MFractors (Xamarin productivity tool) working folder 330 | .mfractor/ 331 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15.3) 2 | project(win_camera) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(NOT CMAKE_CXX_STANDARD_REQUIRED) 15 | set(CMAKE_CXX_STANDARD_REQUIRED TRUE) 16 | endif() 17 | 18 | if(NOT CMAKE_CXX_EXTENSIONS) 19 | set(CMAKE_CXX_EXTENSIONS FALSE) 20 | endif() 21 | 22 | if(MSVC) 23 | add_compile_definitions( 24 | NOMINMAX 25 | ) 26 | endif() 27 | 28 | find_package(ament_cmake REQUIRED) 29 | 30 | find_package(std_msgs REQUIRED) 31 | find_package(sensor_msgs REQUIRED) 32 | find_package(rclcpp REQUIRED) 33 | find_package(rclcpp_components REQUIRED) 34 | find_package(image_transport REQUIRED) 35 | find_package(camera_info_manager REQUIRED) 36 | find_package(compressed_image_transport REQUIRED) 37 | 38 | find_package(rosidl_default_generators REQUIRED) 39 | 40 | rosidl_generate_interfaces(${PROJECT_NAME}_msgs 41 | "msg/MFSample.msg" 42 | DEPENDENCIES std_msgs 43 | ) 44 | 45 | include_directories( 46 | include 47 | ) 48 | 49 | ## Declare a cpp library 50 | add_library(win_camera SHARED 51 | src/wincapture.cpp 52 | src/win_camera_node.cpp 53 | ) 54 | 55 | target_compile_definitions(win_camera PRIVATE "WIN_CAMERA_BUILDING_LIBRARY") 56 | 57 | rclcpp_components_register_nodes( 58 | win_camera 59 | "win_camera::CameraDriver" 60 | ) 61 | 62 | target_link_libraries(win_camera 63 | mf 64 | mfplat 65 | mfuuid 66 | mfreadwrite 67 | Mfsensorgroup 68 | shlwapi 69 | runtimeobject 70 | ) 71 | 72 | ament_target_dependencies(win_camera 73 | compressed_image_transport 74 | image_transport 75 | camera_info_manager 76 | sensor_msgs 77 | rclcpp 78 | ) 79 | 80 | ## Declare a cpp executable 81 | add_executable(win_camera_node src/main.cpp) 82 | 83 | target_link_libraries(win_camera_node 84 | win_camera 85 | ) 86 | 87 | ament_target_dependencies(win_camera_node 88 | compressed_image_transport 89 | image_transport 90 | camera_info_manager 91 | sensor_msgs 92 | rclcpp 93 | ) 94 | 95 | ############# 96 | ## Install ## 97 | ############# 98 | 99 | ## Mark executables and/or libraries for installation 100 | install(TARGETS win_camera 101 | ARCHIVE DESTINATION lib 102 | LIBRARY DESTINATION lib 103 | RUNTIME DESTINATION bin 104 | ) 105 | 106 | install(TARGETS win_camera_node 107 | RUNTIME DESTINATION lib/${PROJECT_NAME} 108 | ) 109 | 110 | install(DIRECTORY include 111 | DESTINATION include 112 | ) 113 | 114 | install(DIRECTORY 115 | launch 116 | DESTINATION share/${PROJECT_NAME}/ 117 | ) 118 | 119 | install(DIRECTORY 120 | rviz 121 | DESTINATION share/${PROJECT_NAME}/ 122 | ) 123 | 124 | install(DIRECTORY 125 | camera_info 126 | DESTINATION share/${PROJECT_NAME}/ 127 | ) 128 | 129 | ament_package() 130 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS 2 Windows Media Foundation Camera Driver 2 | 3 | This ROS node uses Windows Media Foundation's frame server to efficiently process camera frames. 4 | This node uses MF SourceReader API to read frames from camera. The node chooses first available video stream from the camera. Most USB cameras have only one video stream. 5 | Node publishes using image_transport Camera publisher on `image_raw` topic. 6 | 7 | ## System Requirement 8 | 9 | * Microsoft Windows 10 64-bit 10 | * ROS2 Installation (`Foxy` is recommended.) 11 | 12 | ## Getting Started 13 | 14 | To run this driver, a camera will be required to be installed and ready to use on your system. 15 | 16 | You can begin with the below launch file. It will bring up RViz tool where you can see the image stream from your camera. 17 | 18 | ```Batchfile 19 | ros2 launch win_camera win_camera.launch.py 20 | ``` 21 | 22 | In addtions, this driver is registered as a ROS 2 component and it can be running inside a [component container](https://index.ros.org/doc/ros2/Tutorials/Composition/). The below is a demonstration for the usage. 23 | 24 | ```Batchfile 25 | ros2 launch win_camera win_camera_components.launch.py 26 | ``` 27 | 28 | ## Published Topics 29 | 30 | * `/image_raw` (sensor_msgs/msg/Image) 31 | > The image stream from the camera. 32 | 33 | ## Parameters 34 | 35 | * `~image_width` (integer, default: `640`) 36 | > Desired capture image width. 37 | 38 | * `~image_height` (integer, default: `480`) 39 | > Desired capture image height. 40 | 41 | * `~frame_rate` (float, default: `30.0`) 42 | > Desired capture frame rate. 43 | 44 | * `~videoDeviceId` (string, default: ``) 45 | > Symbolic link to the camera to open. if not set default is the first enumerated camera on the system. 46 | 47 | * `~camera_info_url` (string, default: ``) 48 | > Url to the yaml file with camera distrortion parameters. 49 | 50 | 51 | ## Remarks 52 | 53 | This source also contains camera components to enable sharing IMFSample pointers directly into another component container with the same process to enable zero copy and also share GPU surfaces. 54 | This is enabled by using the MFSample Publisher which published IMFSample pointer via a custom msg. This path is experimental. 55 | 56 | # Contributing 57 | 58 | This project welcomes contributions and suggestions. Most contributions require you to agree to a 59 | Contributor License Agreement (CLA) declaring that you have the right to, and actually do, grant us 60 | the rights to use your contribution. For details, visit https://cla.microsoft.com. 61 | 62 | When you submit a pull request, a CLA-bot will automatically determine whether you need to provide 63 | a CLA and decorate the PR appropriately (e.g., label, comment). Simply follow the instructions 64 | provided by the bot. You will only need to do this once across all repos using our CLA. 65 | 66 | This project has adopted the [Microsoft Open Source Code of Conduct](https://opensource.microsoft.com/codeofconduct/). 67 | For more information see the [Code of Conduct FAQ](https://opensource.microsoft.com/codeofconduct/faq/) or 68 | contact [opencode@microsoft.com](mailto:opencode@microsoft.com) with any additional questions or comments. 69 | -------------------------------------------------------------------------------- /camera_info/camera.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1280 2 | image_height: 720 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [944.012173, 0.000000, 534.248944, 0.000000, 893.428920, 358.594765, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.260086, -0.025048, 0.089063, 0.138628, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [852.395142, 0.000000, 565.897630, 0.000000, 0.000000, 922.066223, 386.586250, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /include/win_camera/visibility_control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef WIN_CAMERA__VISIBILITY_CONTROL_H_ 2 | #define WIN_CAMERA__VISIBILITY_CONTROL_H_ 3 | 4 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 5 | // https://gcc.gnu.org/wiki/Visibility 6 | 7 | #if defined _WIN32 || defined __CYGWIN__ 8 | #ifdef __GNUC__ 9 | #define WIN_CAMERA_EXPORT __attribute__ ((dllexport)) 10 | #define WIN_CAMERA_IMPORT __attribute__ ((dllimport)) 11 | #else 12 | #define WIN_CAMERA_EXPORT __declspec(dllexport) 13 | #define WIN_CAMERA_IMPORT __declspec(dllimport) 14 | #endif 15 | #ifdef WIN_CAMERA_BUILDING_LIBRARY 16 | #define WIN_CAMERA_PUBLIC WIN_CAMERA_EXPORT 17 | #else 18 | #define WIN_CAMERA_PUBLIC WIN_CAMERA_IMPORT 19 | #endif 20 | #define WIN_CAMERA_PUBLIC_TYPE WIN_CAMERA_PUBLIC 21 | #define WIN_CAMERA_LOCAL 22 | #else 23 | #define WIN_CAMERA_EXPORT __attribute__ ((visibility("default"))) 24 | #define WIN_CAMERA_IMPORT 25 | #if __GNUC__ >= 4 26 | #define WIN_CAMERA_PUBLIC __attribute__ ((visibility("default"))) 27 | #define WIN_CAMERA_LOCAL __attribute__ ((visibility("hidden"))) 28 | #else 29 | #define WIN_CAMERA_PUBLIC 30 | #define WIN_CAMERA_LOCAL 31 | #endif 32 | #define WIN_CAMERA_PUBLIC_TYPE 33 | #endif 34 | 35 | #endif // WIN_CAMERA__VISIBILITY_CONTROL_H_ -------------------------------------------------------------------------------- /include/win_camera/win_camera_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Microsoft Corporation. All rights reserved. 2 | 3 | #ifndef WIN_CAMERA__WIN_CAMERA_NODE_HPP_ 4 | #define WIN_CAMERA__WIN_CAMERA_NODE_HPP_ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace win_camera 22 | { 23 | 24 | class WIN_CAMERA_PUBLIC_TYPE CameraDriver final 25 | : public rclcpp::Node 26 | { 27 | public: 28 | /// \brief Default constructor 29 | explicit CameraDriver(const rclcpp::NodeOptions & options); 30 | ~CameraDriver(); 31 | 32 | private: 33 | void StartCameraStreaming(); 34 | void StopCameraStreaming(); 35 | void OnSample(IMFSample *pSample, UINT32 u32Width, UINT32 u32Height); 36 | void RescaleCameraInfo(sensor_msgs::msg::CameraInfo &cameraInfo, int width, int height); 37 | 38 | std::shared_ptr camera_info_manager_; 39 | winrt::com_ptr camera_; 40 | std::string frame_id_; 41 | int32_t image_width_; 42 | int32_t image_height_; 43 | float frame_rate_; 44 | int32_t pub_queue_size_; 45 | std::string video_source_path_; 46 | bool isDevice; 47 | std::string camera_info_url_; 48 | std::condition_variable event_finish_; 49 | bool m_bRescaleCameraInfo; 50 | image_transport::CameraPublisher m_cameraPublisher; 51 | }; 52 | 53 | } // namespace win_camera 54 | 55 | #endif // WIN_CAMERA__WIN_CAMERA_NODE_HPP_ -------------------------------------------------------------------------------- /include/win_camera/wincapture.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Microsoft Corporation. All rights reserved. 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | namespace ros_win_camera 13 | { 14 | 15 | class SRCallBackWrapper : public IMFSourceReaderCallback 16 | { 17 | IMFSourceReaderCallback* m_pCap; 18 | long m_nRefCount; 19 | virtual ~SRCallBackWrapper() = default; 20 | SRCallBackWrapper(IMFSourceReaderCallback* pCap) 21 | :m_nRefCount(1) 22 | { 23 | m_pCap = pCap; 24 | } 25 | public: 26 | static SRCallBackWrapper* CreateWeakRefWrapper(IMFSourceReaderCallback* pRef) 27 | { 28 | return new SRCallBackWrapper(pRef); 29 | } 30 | STDMETHODIMP_(ULONG) AddRef() 31 | { 32 | return InterlockedIncrement(&m_nRefCount); 33 | } 34 | STDMETHODIMP_(ULONG) Release() 35 | { 36 | ULONG uCount = InterlockedDecrement(&m_nRefCount); 37 | if (uCount == 0) 38 | { 39 | delete this; 40 | } 41 | return uCount; 42 | } 43 | // IUnknown methods 44 | STDMETHODIMP QueryInterface(REFIID iid, void** ppv) 45 | { 46 | static const QITAB qit[] = 47 | { 48 | QITABENT(SRCallBackWrapper, IMFSourceReaderCallback), 49 | { 0 }, 50 | }; 51 | return QISearch(this, qit, iid, ppv); 52 | } 53 | 54 | // IMFSourceReaderCallback methods 55 | STDMETHODIMP OnEvent(DWORD dwStreamIndex, IMFMediaEvent* mediaEvt) 56 | { 57 | return m_pCap->OnEvent(dwStreamIndex, mediaEvt); 58 | } 59 | 60 | STDMETHODIMP OnFlush(DWORD dwParam) 61 | { 62 | return m_pCap->OnFlush(dwParam); 63 | } 64 | 65 | STDMETHODIMP OnReadSample(HRESULT hrStatus, DWORD dwStreamIndex, 66 | DWORD dwStreamFlags, LONGLONG llTimestamp, IMFSample* pSample) 67 | { 68 | return m_pCap->OnReadSample(hrStatus, dwStreamIndex, dwStreamFlags, llTimestamp, pSample); 69 | } 70 | 71 | 72 | }; 73 | 74 | class WindowsMFCapture : private IMFSourceReaderCallback 75 | { 76 | public: 77 | static WindowsMFCapture* CreateInstance(bool isDevice, const winrt::hstring& link, bool isController = true); 78 | 79 | static winrt::Windows::Foundation::Collections::IVectorView EnumerateCameraLinks(bool bEnumerateSensorCamera); 80 | 81 | bool ChangeCaptureConfig(int32_t width, int32_t height, float frameRate, GUID preferredVideoSubType, bool bForceConversion = false); 82 | 83 | void GetCaptureConfig(uint32_t& width, uint32_t& height, float& framerate, GUID& videoSubtype); 84 | 85 | // IUnknown methods 86 | STDMETHODIMP QueryInterface(REFIID iid, void** ppv) 87 | { 88 | static const QITAB qit[] = 89 | { 90 | QITABENT(WindowsMFCapture, IMFSourceReaderCallback), 91 | { 0 }, 92 | }; 93 | return QISearch(this, qit, iid, ppv); 94 | } 95 | STDMETHODIMP_(ULONG) AddRef() 96 | { 97 | return InterlockedIncrement(&m_nRefCount); 98 | } 99 | STDMETHODIMP_(ULONG) Release() 100 | { 101 | ULONG uCount = InterlockedDecrement(&m_nRefCount); 102 | if (uCount == 0) 103 | { 104 | delete this; 105 | } 106 | return uCount; 107 | } 108 | winrt::event_token AddSampleHandler(winrt::delegate handler); 109 | void RemoveSampleHandler(winrt::event_token token); 110 | void StartStreaming(); 111 | void StopStreaming(); 112 | private: 113 | // private contructor to force consumers to use allocation on heap using CreateInstance 114 | WindowsMFCapture(bool isDevice, const winrt::hstring& link, bool isController = true); 115 | // private destructor so that destruction is controlled by Release() as we inherit from IUnknown 116 | virtual ~WindowsMFCapture() = default; 117 | void InitCaptureWithDevice(const winrt::hstring& cameraSymbolicLink); 118 | void InitCaptureWithUrl(const winrt::hstring& url); 119 | bool FindMatchingMediaType(IMFMediaType** ppMediaType, int32_t width = 0, int32_t height = 0, float frameRate = 0, GUID preferredVideoSubType = GUID_NULL); 120 | 121 | winrt::com_ptr spMediaSource; 122 | winrt::com_ptr spSourceReader; 123 | UINT32 m_u32Width, m_u32Height; 124 | UINT32 m_u32SourceReaderFlags; 125 | 126 | winrt::event> m_configEvent; 127 | winrt::Windows::Foundation::Collections::IVector m_configEventTokenList; 128 | 129 | winrt::event> m_captureCallbackEvent; 130 | 131 | // IMFSourceReaderCallback methods 132 | STDMETHODIMP OnEvent(DWORD dwStreamIndex, IMFMediaEvent* mediaEvt) 133 | { 134 | MediaEventType evt; 135 | mediaEvt->GetType(&evt); 136 | 137 | return S_OK; 138 | } 139 | 140 | STDMETHODIMP OnFlush(DWORD) 141 | { 142 | EnterCriticalSection(&m_critsec); 143 | m_configEvent(); 144 | for (auto token : m_configEventTokenList) 145 | { 146 | m_configEvent.remove(token); 147 | } 148 | LeaveCriticalSection(&m_critsec); 149 | return S_OK; 150 | } 151 | 152 | STDMETHODIMP OnReadSample(HRESULT hrStatus, DWORD dwStreamIndex, 153 | DWORD dwStreamFlags, LONGLONG llTimestamp, IMFSample* pSample); 154 | 155 | long m_nRefCount; 156 | CRITICAL_SECTION m_critsec; 157 | bool m_bStreamingStarted; 158 | bool m_bIsController; 159 | winrt::slim_mutex m_apiGuardMutex; 160 | winrt::slim_mutex m_sampleHandlerMutex; 161 | }; 162 | 163 | 164 | } -------------------------------------------------------------------------------- /launch/win_camera.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Microsoft Corporation. 2 | # Licensed under the MIT License. 3 | 4 | import os 5 | import launch 6 | import launch.actions 7 | import launch.substitutions 8 | import launch_ros.actions 9 | 10 | from ament_index_python.packages import get_package_share_directory 11 | 12 | 13 | def generate_launch_description(): 14 | share_dir = get_package_share_directory('win_camera') 15 | rviz_default_view = os.path.join(share_dir, 'rviz', 'default_view.rviz') 16 | 17 | return launch.LaunchDescription([ 18 | launch_ros.actions.Node( 19 | package='win_camera', executable='win_camera_node', output='screen', 20 | name=['win_camera'], 21 | parameters=[ 22 | {'frame_rate': 5.0}, 23 | {'frame_id': 'camera'}, 24 | {'image_width': 1280 }, 25 | {'image_height': 720 }, 26 | {'camera_info_url': 'package://win_camera/camera_info/camera.yaml'}, 27 | ], 28 | arguments=[{'--ros-args', '--log-level', 'INFO'}] 29 | ), 30 | 31 | #launch_ros.actions.Node( 32 | # package='tf2_ros', executable='static_transform_publisher', output='screen', 33 | # name=['static_transform_publisher'], 34 | # arguments=[ 35 | # '0.1', '0.2', '0.3', '0.4', '.5', '.6', 'map', 'camera' 36 | # ]), 37 | 38 | # launch_ros.actions.Node( 39 | # package='rviz2', executable='rviz2', output='screen', 40 | # name=['rviz2'], 41 | # arguments=[ 42 | # '-d', rviz_default_view 43 | # ]), 44 | ]) -------------------------------------------------------------------------------- /launch/win_camera_components.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) Microsoft Corporation. 2 | # Licensed under the MIT License. 3 | 4 | import os 5 | import launch 6 | import launch.actions 7 | import launch.substitutions 8 | import launch_ros.actions 9 | 10 | from launch_ros.actions import ComposableNodeContainer 11 | from launch_ros.descriptions import ComposableNode 12 | from ament_index_python.packages import get_package_share_directory 13 | 14 | def generate_launch_description(): 15 | share_dir = get_package_share_directory('win_camera') 16 | rviz_default_view = os.path.join(share_dir, 'rviz', 'default_view.rviz') 17 | container = ComposableNodeContainer( 18 | name='my_container', 19 | namespace='', 20 | package='rclcpp_components', 21 | executable='component_container', 22 | composable_node_descriptions=[ 23 | ComposableNode( 24 | package='win_camera', 25 | plugin='win_camera::CameraDriver', 26 | name='win_camera_comp', 27 | parameters=[ 28 | {'frame_rate': 5.0}, 29 | {'frame_id': 'camera'}, 30 | {'camera_info_url': 'package://win_camera/camera_info/camera.yaml'}, 31 | ]), 32 | ], 33 | output='screen', 34 | ) 35 | 36 | return launch.LaunchDescription([ 37 | container, 38 | launch_ros.actions.Node( 39 | package='tf2_ros', executable='static_transform_publisher', output='screen', 40 | name=['static_transform_publisher'], 41 | arguments=[ 42 | '0.1', '0.2', '0.3', '0.4', '.5', '.6', 'map', 'camera' 43 | ]), 44 | launch_ros.actions.Node( 45 | package='rviz2', executable='rviz2', output='screen', 46 | name=['rviz2'], 47 | arguments=[ 48 | '-d', rviz_default_view 49 | ]), 50 | ]) -------------------------------------------------------------------------------- /msg/MFSample.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | int64 sample -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | win_camera 7 | 0.2.0 8 | win_camera uses Windows Media Foundation to capture camera image. 9 | 10 | Sudhanshu Sohoni 11 | MIT 12 | 13 | ament_cmake 14 | 15 | rosidl_default_generators 16 | rosidl_default_runtime 17 | rosidl_interface_packages 18 | 19 | image_transport 20 | rclcpp 21 | sensor_msgs 22 | camera_info_manager 23 | compressed_image_transport 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rviz/default_view.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 176 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Camera1 10 | - /Camera1/Status1 11 | Splitter Ratio: 0.5 12 | Tree Height: 1067 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | Visualization Manager: 27 | Class: "" 28 | Displays: 29 | - Alpha: 0.5 30 | Cell Size: 1 31 | Class: rviz_default_plugins/Grid 32 | Color: 160; 160; 164 33 | Enabled: true 34 | Line Style: 35 | Line Width: 0.029999999329447746 36 | Value: Lines 37 | Name: Grid 38 | Normal Cell Count: 0 39 | Offset: 40 | X: 0 41 | Y: 0 42 | Z: 0 43 | Plane: XY 44 | Plane Cell Count: 10 45 | Reference Frame: 46 | Value: true 47 | - Class: rviz_default_plugins/Camera 48 | Enabled: true 49 | Image Rendering: background and overlay 50 | Name: Camera 51 | Overlay Alpha: 0.5 52 | Topic: 53 | Depth: 5 54 | Durability Policy: Volatile 55 | History Policy: Keep Last 56 | Reliability Policy: Reliable 57 | Value: /image_raw 58 | Value: true 59 | Visibility: 60 | Grid: true 61 | Image: true 62 | Value: true 63 | Zoom Factor: 1 64 | Enabled: true 65 | Global Options: 66 | Background Color: 48; 48; 48 67 | Fixed Frame: map 68 | Frame Rate: 30 69 | Name: root 70 | Tools: 71 | - Class: rviz_default_plugins/Interact 72 | Hide Inactive Objects: true 73 | - Class: rviz_default_plugins/MoveCamera 74 | - Class: rviz_default_plugins/Select 75 | - Class: rviz_default_plugins/FocusCamera 76 | - Class: rviz_default_plugins/Measure 77 | Line color: 128; 128; 0 78 | - Class: rviz_default_plugins/SetInitialPose 79 | Topic: 80 | Depth: 5 81 | Durability Policy: Volatile 82 | History Policy: Keep Last 83 | Reliability Policy: Reliable 84 | Value: /initialpose 85 | - Class: rviz_default_plugins/SetGoal 86 | Topic: 87 | Depth: 5 88 | Durability Policy: Volatile 89 | History Policy: Keep Last 90 | Reliability Policy: Reliable 91 | Value: /goal_pose 92 | - Class: rviz_default_plugins/PublishPoint 93 | Single click: true 94 | Topic: 95 | Depth: 5 96 | Durability Policy: Volatile 97 | History Policy: Keep Last 98 | Reliability Policy: Reliable 99 | Value: /clicked_point 100 | Transformation: 101 | Current: 102 | Class: rviz_default_plugins/TF 103 | Value: true 104 | Views: 105 | Current: 106 | Class: rviz_default_plugins/Orbit 107 | Distance: 10 108 | Enable Stereo Rendering: 109 | Stereo Eye Separation: 0.05999999865889549 110 | Stereo Focal Distance: 1 111 | Swap Stereo Eyes: false 112 | Value: false 113 | Focal Point: 114 | X: 0 115 | Y: 0 116 | Z: 0 117 | Focal Shape Fixed Size: true 118 | Focal Shape Size: 0.05000000074505806 119 | Invert Z Axis: false 120 | Name: Current View 121 | Near Clip Distance: 0.009999999776482582 122 | Pitch: 0.785398006439209 123 | Target Frame: 124 | Value: Orbit (rviz) 125 | Yaw: 0.785398006439209 126 | Saved: ~ 127 | Window Geometry: 128 | Camera: 129 | collapsed: false 130 | Displays: 131 | collapsed: false 132 | Height: 2004 133 | Hide Left Dock: false 134 | Hide Right Dock: false 135 | Image: 136 | collapsed: false 137 | QMainWindow State: 000000ff00000000fd00000004000000000000031d00000729fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000d500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000007700000558000001dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000005d9000000ce0000005100fffffffb0000000a0049006d00610067006501000006b1000000ef0000005100ffffff000000010000025300000729fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000077000007290000016f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000097c0000072900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 138 | Selection: 139 | collapsed: false 140 | Tool Properties: 141 | collapsed: false 142 | Views: 143 | collapsed: false 144 | Width: 3840 145 | X: -2 146 | Y: -16 147 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Microsoft Corporation. All rights reserved. 2 | #include 3 | 4 | int main(int argc, char** argv) 5 | { 6 | rclcpp::init(argc, argv); 7 | 8 | rclcpp::spin( 9 | std::make_shared( 10 | rclcpp::NodeOptions())); 11 | rclcpp::shutdown(); 12 | 13 | return 0; 14 | } 15 | -------------------------------------------------------------------------------- /src/win_camera_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Microsoft Corporation. All rights reserved. 2 | #include 3 | 4 | using namespace winrt::Windows::System::Threading; 5 | using namespace winrt::Windows::Foundation; 6 | using namespace ros_win_camera; 7 | constexpr int32_t PUBLISHER_QUEUE_SIZE = 4; 8 | constexpr int32_t DEFAULT_WIDTH = 640; 9 | constexpr int32_t DEFAULT_HEIGHT = 480; 10 | constexpr float DEFAULT_FRAMERATE = 30.0; 11 | auto videoFormat = MFVideoFormat_ARGB32; 12 | 13 | namespace win_camera 14 | { 15 | 16 | CameraDriver::CameraDriver(const rclcpp::NodeOptions & options) 17 | : rclcpp::Node("win_camera_node", options), 18 | isDevice(true) 19 | { 20 | this->declare_parameter("frame_id", "camera"); 21 | this->declare_parameter("frame_rate", DEFAULT_FRAMERATE); 22 | this->declare_parameter("pub_queue_size", PUBLISHER_QUEUE_SIZE); 23 | this->declare_parameter("image_width", DEFAULT_WIDTH); 24 | this->declare_parameter("image_height", DEFAULT_HEIGHT); 25 | this->declare_parameter("videoDeviceId", ""); 26 | this->declare_parameter("videoUrl", ""); 27 | this->declare_parameter("camera_info_url", ""); 28 | this->declare_parameter("rescale_camera_info", false); 29 | 30 | 31 | this->get_parameter("frame_id", frame_id_); 32 | this->get_parameter("frame_rate", frame_rate_); 33 | this->get_parameter("pub_queue_size", pub_queue_size_); 34 | this->get_parameter("image_width", image_width_); 35 | this->get_parameter("image_height", image_height_); 36 | this->get_parameter("videoDeviceId", video_source_path_); 37 | this->get_parameter("camera_info_url", camera_info_url_); 38 | this->get_parameter("rescale_camera_info", m_bRescaleCameraInfo); 39 | if (video_source_path_.empty()) 40 | { 41 | this->get_parameter("videoUrl", video_source_path_); 42 | if (!video_source_path_.empty()) 43 | { 44 | isDevice = false; 45 | } 46 | else 47 | { 48 | int i = 0; 49 | // no source path is specified; we default to first enumerated camera 50 | video_source_path_ = winrt::to_string(ros_win_camera::WindowsMFCapture::EnumerateCameraLinks(false).GetAt(i)); 51 | } 52 | } 53 | else if (video_source_path_ == "enumerate") 54 | { 55 | int i = 0; 56 | RCLCPP_INFO(this->get_logger(), "Available Cameras:"); 57 | 58 | auto links = ros_win_camera::WindowsMFCapture::EnumerateCameraLinks(false); 59 | for (auto sym : links) 60 | { 61 | RCLCPP_INFO(this->get_logger(), "%d. %s", ++i, winrt::to_string(sym).c_str()); 62 | } 63 | 64 | if (links.Size() > 0) 65 | { 66 | video_source_path_ = winrt::to_string(links.GetAt(0)); 67 | } 68 | } 69 | 70 | camera_info_manager_ = std::make_shared(this, frame_id_, ""); 71 | if (!camera_info_url_.empty()) 72 | { 73 | if (camera_info_manager_->validateURL(camera_info_url_)) 74 | { 75 | RCLCPP_INFO(this->get_logger(), "validated Camera info url: %s", camera_info_url_.c_str()); 76 | camera_info_manager_->loadCameraInfo(camera_info_url_); 77 | } 78 | } 79 | 80 | m_cameraPublisher = image_transport::create_camera_publisher(this, "image_raw"); 81 | 82 | StartCameraStreaming(); 83 | } 84 | 85 | CameraDriver::~CameraDriver() 86 | { 87 | StopCameraStreaming(); 88 | } 89 | 90 | void CameraDriver::StartCameraStreaming() 91 | { 92 | bool resChangeInProgress = false; 93 | auto rosImagePubHandler = [this, &resChangeInProgress](winrt::hresult_error ex, winrt::hstring msg, IMFSample* pSample) 94 | { 95 | if (pSample) 96 | { 97 | auto info = this->camera_info_manager_->getCameraInfo(); 98 | if (((info.height != this->image_height_) || (info.width != this->image_width_)) && info.height && info.width && (!resChangeInProgress)) 99 | { 100 | resChangeInProgress = true; 101 | ThreadPool::RunAsync([this, &resChangeInProgress](IAsyncAction) 102 | { 103 | auto info = this->camera_info_manager_->getCameraInfo(); 104 | if (this->camera_->ChangeCaptureConfig(info.width, info.height, this->frame_rate_, videoFormat, true)) 105 | { 106 | this->image_height_ = info.height; 107 | this->image_width_ = info.width; 108 | resChangeInProgress = false; 109 | } 110 | else 111 | { 112 | RCLCPP_WARN(this->get_logger(), "Setting resolution failed. Use rescale_camera_info param for rescaling"); 113 | } 114 | }); 115 | } 116 | else 117 | { 118 | OnSample(pSample, (UINT32)this->image_width_, (UINT32)this->image_height_); 119 | } 120 | } 121 | else 122 | { 123 | if ((HRESULT)ex.code().value == MF_E_END_OF_STREAM) 124 | { 125 | RCLCPP_INFO(this->get_logger(), "EOS"); 126 | } 127 | this->event_finish_.notify_all(); 128 | } 129 | }; 130 | 131 | camera_.attach(WindowsMFCapture::CreateInstance(isDevice, winrt::to_hstring(video_source_path_), true)); 132 | camera_->ChangeCaptureConfig(image_width_, image_height_, frame_rate_, videoFormat, true); 133 | camera_->StartStreaming(); 134 | camera_->AddSampleHandler(rosImagePubHandler); 135 | } 136 | 137 | void CameraDriver::StopCameraStreaming() 138 | { 139 | std::mutex mutexFinish; 140 | std::unique_lock lockFinish(mutexFinish); 141 | ThreadPool::RunAsync([this](IAsyncAction) 142 | { 143 | this->camera_->StopStreaming(); 144 | }); 145 | event_finish_.wait(lockFinish); 146 | } 147 | 148 | void CameraDriver::OnSample(IMFSample *pSample, UINT32 u32Width, UINT32 u32Height) 149 | { 150 | try 151 | { 152 | if (pSample) 153 | { 154 | winrt::com_ptr spMediaBuf; 155 | winrt::com_ptr spMediaBuf2d; 156 | uint32_t littleEndian = 1; 157 | BYTE* pix; 158 | LONG Stride; 159 | 160 | auto cameraInfo = camera_info_manager_->getCameraInfo(); 161 | if (cameraInfo.height == 0 && cameraInfo.width == 0) 162 | { 163 | cameraInfo.height = u32Height; 164 | cameraInfo.width = u32Width; 165 | } 166 | else if (cameraInfo.height != u32Height || cameraInfo.width != u32Width) 167 | { 168 | if (m_bRescaleCameraInfo) 169 | { 170 | int old_width = cameraInfo.width; 171 | int old_height = cameraInfo.height; 172 | RescaleCameraInfo(cameraInfo, u32Width, u32Height); 173 | RCLCPP_INFO_ONCE(this->get_logger(), "Camera calibration automatically rescaled from %dx%d to %dx%d", 174 | old_width, old_height, u32Width, u32Height); 175 | } 176 | else 177 | { 178 | RCLCPP_WARN_ONCE(this->get_logger(), "Calibration resolution %dx%d does not match camera resolution %dx%d. " 179 | "Use rescale_camera_info param for rescaling", 180 | cameraInfo.width, cameraInfo.height, u32Width, u32Height); 181 | } 182 | } 183 | cameraInfo.header.stamp = rclcpp::Time(); 184 | cameraInfo.header.frame_id = frame_id_; 185 | 186 | winrt::check_hresult(pSample->GetBufferByIndex(0, spMediaBuf.put())); 187 | spMediaBuf2d = spMediaBuf.as(); 188 | 189 | auto ros_image = std::make_shared(); 190 | 191 | ros_image->header = cameraInfo.header; 192 | ros_image->height = u32Height; 193 | ros_image->width = u32Width; 194 | ros_image->encoding = sensor_msgs::image_encodings::BGRA8; 195 | ros_image->is_bigendian = !*((uint8_t*)&littleEndian); 196 | 197 | winrt::check_hresult(spMediaBuf2d->Lock2D(&pix, &Stride)); 198 | if (Stride < 0) 199 | { 200 | ros_image->step = -Stride; 201 | size_t size = ros_image->step * u32Height; 202 | ros_image->data.resize(size); 203 | for (int i = 0; i < u32Height; i++) 204 | { 205 | memcpy(ros_image->data.data(), pix + Stride * i, -Stride); 206 | } 207 | } 208 | else 209 | { 210 | ros_image->step = Stride; 211 | size_t size = Stride * u32Height; 212 | ros_image->data.resize(size); 213 | memcpy(ros_image->data.data(), pix, size); 214 | 215 | } 216 | winrt::check_hresult(spMediaBuf2d->Unlock2D()); 217 | m_cameraPublisher.publish(*ros_image, cameraInfo); 218 | } 219 | } 220 | catch (winrt::hresult_error const& ex) 221 | { 222 | std::cout << ex.code() << ex.message().c_str(); 223 | } 224 | } 225 | 226 | void CameraDriver::RescaleCameraInfo(sensor_msgs::msg::CameraInfo &cameraInfo, int width, int height) 227 | { 228 | double widthCoeff = static_cast(width) / cameraInfo.width; 229 | double heightCoeff = static_cast(height) / cameraInfo.height; 230 | cameraInfo.width = width; 231 | cameraInfo.height = height; 232 | 233 | // ref: https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/CameraInfo.msg 234 | cameraInfo.k[0] *= widthCoeff; 235 | cameraInfo.k[2] *= widthCoeff; 236 | cameraInfo.k[4] *= heightCoeff; 237 | cameraInfo.k[5] *= heightCoeff; 238 | 239 | cameraInfo.p[0] *= widthCoeff; 240 | cameraInfo.p[2] *= widthCoeff; 241 | cameraInfo.p[5] *= heightCoeff; 242 | cameraInfo.p[6] *= heightCoeff; 243 | } 244 | 245 | } // namespace win_camera 246 | 247 | #include "rclcpp_components/register_node_macro.hpp" 248 | 249 | RCLCPP_COMPONENTS_REGISTER_NODE(win_camera::CameraDriver) 250 | -------------------------------------------------------------------------------- /src/wincapture.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Microsoft Corporation. All rights reserved. 2 | #include 3 | 4 | using namespace winrt; 5 | using namespace winrt::Windows::Foundation; 6 | using namespace winrt::Windows::Foundation::Collections; 7 | using namespace winrt::Windows::Devices::Enumeration; 8 | using namespace winrt::Windows::System::Threading; 9 | #define _INFO printf 10 | #define _WARN printf 11 | #define _ERROR printf 12 | #define LOG_TRANSFORMS() \ 13 | {\ 14 | int i = 0;\ 15 | while (MF_E_INVALIDINDEX != spSourceReader.try_as()->GetTransformForStream(MF_SOURCE_READER_FIRST_VIDEO_STREAM, i++, &guid, spTransform.put()))\ 16 | {\ 17 | _INFO("\nTranform %d: %x-%x-%x-%x%x%x%x", i - 1, guid.Data1, guid.Data2, guid.Data3, guid.Data4[0], guid.Data4[1], guid.Data4[2], guid.Data4[3]);\ 18 | }\ 19 | _INFO("\nNumber of tranforms in Chain: %d", i - 1);\ 20 | } 21 | 22 | namespace ros_win_camera 23 | { 24 | WindowsMFCapture::WindowsMFCapture(bool isDevice, const winrt::hstring& link, bool isController /*=true*/) 25 | :m_nRefCount(1), 26 | m_u32SourceReaderFlags(0), 27 | m_u32Height(480), 28 | m_u32Width(640), 29 | m_bStreamingStarted(false), 30 | m_bIsController(isController) 31 | { 32 | InitializeCriticalSection(&m_critsec); 33 | m_configEventTokenList = single_threaded_vector(); 34 | 35 | if (isDevice) 36 | { 37 | InitCaptureWithDevice(link); 38 | } 39 | else 40 | { 41 | InitCaptureWithUrl(link); 42 | } 43 | } 44 | winrt::event_token WindowsMFCapture::AddSampleHandler(winrt::delegate handler) 45 | { 46 | std::lock_guard g(m_apiGuardMutex); 47 | 48 | auto tok = m_captureCallbackEvent.add(handler); 49 | return tok; 50 | } 51 | void WindowsMFCapture::RemoveSampleHandler(winrt::event_token token) 52 | { 53 | std::lock_guard g(m_apiGuardMutex); 54 | m_captureCallbackEvent.remove(token); 55 | } 56 | winrt::Windows::Foundation::Collections::IVectorView WindowsMFCapture::EnumerateCameraLinks(bool bEnumerateSensorCamera) 57 | { 58 | auto links = single_threaded_vector(); 59 | auto filteredDevices = DeviceInformation::FindAllAsync(DeviceClass::VideoCapture).get(); 60 | if (!filteredDevices.Size()) 61 | { 62 | throw_hresult(MF_E_NO_CAPTURE_DEVICES_AVAILABLE); 63 | } 64 | auto deviceIter = filteredDevices.First(); 65 | while (deviceIter.HasCurrent()) 66 | { 67 | auto device = deviceIter.Current(); 68 | links.Append(device.Id()); 69 | deviceIter.MoveNext(); 70 | } 71 | return links.GetView(); 72 | } 73 | 74 | void WindowsMFCapture::InitCaptureWithDevice(const winrt::hstring& cameraSymbolicLink) 75 | { 76 | 77 | winrt::com_ptr spAttributes; 78 | winrt::com_ptr spSRAttributes; 79 | 80 | winrt::com_ptr spSensorGrp; 81 | winrt::com_ptr spSensorDevice; 82 | 83 | check_hresult(CoInitialize(NULL)); 84 | check_hresult(MFStartup(MF_VERSION)); 85 | 86 | check_hresult(MFCreateSensorGroup(cameraSymbolicLink.c_str(), spSensorGrp.put())); 87 | check_hresult(spSensorGrp->GetSensorDevice(0, spSensorDevice.put())); 88 | check_hresult(spSensorDevice->SetSensorDeviceMode(m_bIsController? MFSensorDeviceMode::MFSensorDeviceMode_Controller : MFSensorDeviceMode::MFSensorDeviceMode_Shared)); 89 | check_hresult(spSensorGrp->CreateMediaSource(spMediaSource.put())); 90 | check_hresult(MFCreateAttributes(spSRAttributes.put(), 3)); 91 | 92 | check_hresult(spSRAttributes->SetUINT32(MF_SOURCE_READER_ENABLE_ADVANCED_VIDEO_PROCESSING, TRUE)); 93 | check_hresult(spSRAttributes->SetUINT32(MF_LOW_LATENCY, TRUE)); 94 | 95 | // use weak ref wrpper here to avoid circular reference 96 | check_hresult(spSRAttributes->SetUnknown(MF_SOURCE_READER_ASYNC_CALLBACK, SRCallBackWrapper::CreateWeakRefWrapper(this))); 97 | 98 | check_hresult(MFCreateSourceReaderFromMediaSource(spMediaSource.get(), spSRAttributes.get(), spSourceReader.put())); 99 | } 100 | 101 | void WindowsMFCapture::StopStreaming() 102 | { 103 | std::lock_guard g(m_apiGuardMutex); 104 | if (!m_bStreamingStarted) return; 105 | 106 | std::condition_variable eventCompletion; 107 | std::mutex m; 108 | std::unique_lock ul(m); 109 | 110 | EnterCriticalSection(&m_critsec); 111 | m_configEventTokenList.Append(m_configEvent.add([&]() 112 | { 113 | m_bStreamingStarted = false; 114 | check_hresult(spSourceReader->SetStreamSelection((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, FALSE)); 115 | eventCompletion.notify_one(); 116 | })); 117 | LeaveCriticalSection(&m_critsec); 118 | check_hresult(spSourceReader->Flush(MF_SOURCE_READER_FIRST_VIDEO_STREAM)); 119 | 120 | // wait for the config-stop event to complete 121 | eventCompletion.wait(ul); 122 | 123 | _INFO("\nStopped streaming complete!\n"); 124 | m_captureCallbackEvent(hresult_error(MF_E_END_OF_STREAM), L"Sample Stopped", nullptr); 125 | } 126 | 127 | void WindowsMFCapture::StartStreaming() 128 | { 129 | std::lock_guard g(m_apiGuardMutex); 130 | if (m_bStreamingStarted) return; 131 | 132 | winrt::com_ptr spMT; 133 | GUID subtype; 134 | 135 | check_hresult(spSourceReader->SetStreamSelection((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, TRUE)); 136 | check_hresult(spSourceReader->ReadSample(MF_SOURCE_READER_FIRST_VIDEO_STREAM, m_u32SourceReaderFlags, NULL, NULL, NULL, NULL)); 137 | m_bStreamingStarted = true; 138 | } 139 | 140 | void WindowsMFCapture::InitCaptureWithUrl(const winrt::hstring& url) 141 | { 142 | winrt::com_ptr spSRAttributes; 143 | GUID subtype; 144 | 145 | check_hresult(CoInitialize(NULL)); 146 | check_hresult(MFStartup(MF_VERSION)); 147 | 148 | check_hresult(MFCreateAttributes(spSRAttributes.put(), 3)); 149 | check_hresult(spSRAttributes->SetUINT32(MF_SOURCE_READER_ENABLE_ADVANCED_VIDEO_PROCESSING, TRUE)); 150 | check_hresult(spSRAttributes->SetUINT32(MF_LOW_LATENCY, TRUE)); 151 | check_hresult(spSRAttributes->SetUnknown(MF_SOURCE_READER_ASYNC_CALLBACK, SRCallBackWrapper::CreateWeakRefWrapper(this))); 152 | check_hresult(MFCreateSourceReaderFromURL(url.c_str(), spSRAttributes.get(), spSourceReader.put())); 153 | m_u32Width = 640; 154 | m_u32Height = 480; 155 | } 156 | 157 | HRESULT WindowsMFCapture::OnReadSample( 158 | HRESULT hrStatus, 159 | DWORD /* dwStreamIndex */, 160 | DWORD dwStreamFlags, 161 | LONGLONG llTimestamp, 162 | IMFSample* pSample // Can be NULL 163 | ) 164 | { 165 | std::lock_guard g(m_sampleHandlerMutex); 166 | try 167 | { 168 | if (SUCCEEDED(hrStatus)) 169 | { 170 | if (!pSample && !dwStreamFlags) 171 | { 172 | //Drain completed 173 | winrt::com_ptr spMT; 174 | GUID subtype; 175 | m_u32SourceReaderFlags = 0; 176 | check_hresult(spSourceReader->GetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, MF_SOURCE_READER_CURRENT_TYPE_INDEX, spMT.put())); 177 | 178 | check_hresult(spMT->GetGUID(MF_MT_SUBTYPE, &subtype)); 179 | if (!IsEqualGUID(subtype, MFVideoFormat_ARGB32)) 180 | { 181 | check_hresult(spMT->SetGUID(MF_MT_SUBTYPE, MFVideoFormat_ARGB32)); 182 | check_hresult(spSourceReader->SetCurrentMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, nullptr, spMT.get())); 183 | } 184 | check_hresult(MFGetAttributeSize(spMT.get(), MF_MT_FRAME_SIZE, &m_u32Width, &m_u32Height)); 185 | 186 | int i = 0; 187 | GUID guid; 188 | winrt::com_ptr spTransform; 189 | while (MF_E_INVALIDINDEX != spSourceReader.try_as()->GetTransformForStream(MF_SOURCE_READER_FIRST_VIDEO_STREAM, i++, &guid, spTransform.put())) 190 | { 191 | _INFO("Tranform %d: %x-%x-%x-%x%x%x%x", i - 1, guid.Data1, guid.Data2, guid.Data3, guid.Data4[0], guid.Data4[1], guid.Data4[2], guid.Data4[3]); 192 | } 193 | _INFO("Aftermath:Number of tranforms in Chain: %d", i - 1); 194 | 195 | } 196 | } 197 | else 198 | { 199 | throw_hresult(hrStatus); 200 | } 201 | if (MF_SOURCE_READERF_NATIVEMEDIATYPECHANGED & dwStreamFlags) 202 | { 203 | winrt::com_ptr spMT; 204 | check_hresult(spSourceReader->GetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, MF_SOURCE_READER_CURRENT_TYPE_INDEX, spMT.put())); 205 | check_hresult(MFGetAttributeSize(spMT.get(), MF_MT_FRAME_SIZE, &m_u32Width, &m_u32Height)); 206 | _INFO("\nNative Type changed: %dx%d", m_u32Width, m_u32Height); 207 | } 208 | if (MF_SOURCE_READERF_CURRENTMEDIATYPECHANGED & dwStreamFlags) 209 | { 210 | winrt::com_ptr spMT; 211 | check_hresult(spSourceReader->GetCurrentMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, spMT.put())); 212 | check_hresult(MFGetAttributeSize(spMT.get(), MF_MT_FRAME_SIZE, &m_u32Width, &m_u32Height)); 213 | _INFO("\nCurrent Type changed %dx%d", m_u32Width, m_u32Height); 214 | GUID guid; 215 | winrt::com_ptr spTransform; 216 | LOG_TRANSFORMS(); 217 | 218 | m_u32SourceReaderFlags = MF_SOURCE_READER_CONTROLF_DRAIN; 219 | 220 | } 221 | if (MF_SOURCE_READERF_ENDOFSTREAM & dwStreamFlags) 222 | { 223 | m_captureCallbackEvent(hresult_error(MF_E_END_OF_STREAM), L"End Of stream",nullptr); 224 | } 225 | } 226 | catch (hresult_error const& ex) 227 | { 228 | _ERROR("%x:%s", (unsigned int)ex.code(), winrt::to_string(ex.message()).c_str()); 229 | m_captureCallbackEvent(ex, L":Trying to read sample in callback", nullptr); 230 | } 231 | if (m_bStreamingStarted) 232 | { 233 | if (SUCCEEDED(hrStatus) && pSample) 234 | { 235 | m_captureCallbackEvent(hresult_error(S_OK), L"Sample received", pSample); 236 | } 237 | 238 | HRESULT hr = spSourceReader->ReadSample(MF_SOURCE_READER_FIRST_VIDEO_STREAM, m_u32SourceReaderFlags, NULL, NULL, NULL, NULL); 239 | if (hr == MF_E_NOTACCEPTING) 240 | { 241 | m_captureCallbackEvent(hresult_error(S_OK), L"Flush in progress", nullptr); 242 | } 243 | else if (FAILED(hr)) 244 | { 245 | hresult_error ex(hr); 246 | _ERROR("%x:%s", (unsigned int)ex.code(), winrt::to_string(ex.message()).c_str()); 247 | m_captureCallbackEvent(ex, L":Trying to read sample in callback", nullptr); 248 | } 249 | } 250 | return S_OK; 251 | } 252 | bool WindowsMFCapture::FindMatchingMediaType(IMFMediaType** ppMediaType, int32_t width/*=0*/, int32_t height/*=0*/, float frameRate/*=0*/, GUID preferredVideoSubType/*=GUID_NULL*/) 253 | { 254 | winrt::com_ptr spMediaType; 255 | int idx = 0; 256 | UINT32 FRNum, FRDen; 257 | bool bMatch = true; 258 | GUID subType = GUID_NULL; 259 | check_pointer(ppMediaType); 260 | while (S_OK == spSourceReader->GetNativeMediaType((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, idx, spMediaType.put())) 261 | { 262 | check_hresult(MFGetAttributeSize(spMediaType.get(), MF_MT_FRAME_SIZE, &m_u32Width, &m_u32Height)); 263 | check_hresult(MFGetAttributeRatio(spMediaType.get(), MF_MT_FRAME_RATE, &FRNum, &FRDen)); 264 | check_hresult(spMediaType->GetGUID(MF_MT_SUBTYPE,&subType)); 265 | float rate = ((float)FRNum / (float)FRDen); 266 | //_INFO("\nGot supported resolution :%dx%d@%d - %x-%x-%x-%x", m_u32Width, m_u32Height, (int)rate, subType.Data1,subType.Data2, subType.Data3,subType.Data4); 267 | bMatch = ((height == 0) || (width == 0) || ((m_u32Width == width) && (height == m_u32Height))); 268 | bMatch = bMatch && (((int)rate == (int)frameRate) || (frameRate == 0)); 269 | bMatch = bMatch && (IsEqualGUID(preferredVideoSubType, subType) || IsEqualGUID(preferredVideoSubType, GUID_NULL)); 270 | if (bMatch) 271 | { 272 | break; 273 | } 274 | idx++; 275 | } 276 | spMediaType.copy_to(ppMediaType); 277 | return bMatch; 278 | 279 | } 280 | void WindowsMFCapture::GetCaptureConfig(uint32_t& width, uint32_t& height, float& framerate, GUID& videoSubtype) 281 | { 282 | std::lock_guard g(m_apiGuardMutex); 283 | winrt::com_ptr spMT; 284 | check_hresult(spSourceReader->GetCurrentMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, spMT.put())); 285 | check_hresult(MFGetAttributeSize(spMT.get(), MF_MT_FRAME_SIZE, &width, &height)); 286 | uint32_t Num, Denom; 287 | check_hresult(MFGetAttributeRatio(spMT.get(), MF_MT_FRAME_RATE, &Num, &Denom)); 288 | framerate = (float)Num / (float)Denom; 289 | check_hresult(spMT->GetGUID(MF_MT_SUBTYPE, &videoSubtype)); 290 | 291 | } 292 | bool WindowsMFCapture::ChangeCaptureConfig(int32_t width, int32_t height, float frameRate, GUID preferredVideoSubType, bool bForceConversion /*= false*/) 293 | { 294 | std::lock_guard g(m_apiGuardMutex); 295 | bool bStatus = true; 296 | std::condition_variable eventCompletion; 297 | std::mutex m; 298 | std::unique_lock ul(m); 299 | auto configHandler = [&]() 300 | { 301 | HRESULT hr = S_OK; 302 | winrt::com_ptr spMediaType; 303 | UINT32 FRNum, FRDen; 304 | int idx = 0; 305 | _INFO("\nSetting resolution :%dx%d@%f", width, height, frameRate); 306 | if (!m_bIsController) 307 | { 308 | _WARN("Instance is not in controller mode. Setting up a conversion"); 309 | HRESULT hr = spSourceReader->GetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, MF_SOURCE_READER_CURRENT_TYPE_INDEX, spMediaType.put()); 310 | 311 | check_hresult(MFSetAttributeSize(spMediaType.get(), MF_MT_FRAME_SIZE, width, height)); 312 | check_hresult(MFSetAttributeRatio(spMediaType.get(), MF_MT_FRAME_RATE, (UINT32)(frameRate*100), 100 )); 313 | check_hresult(spMediaType->SetGUID(MF_MT_SUBTYPE, preferredVideoSubType)); 314 | check_hresult(spSourceReader->SetCurrentMediaType((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, nullptr, spMediaType.get())); 315 | m_u32Width = width; 316 | m_u32Height = height; 317 | 318 | } 319 | else if (!FindMatchingMediaType(spMediaType.put(), width, height, frameRate, preferredVideoSubType)) 320 | { 321 | if (bForceConversion) 322 | { 323 | _WARN("No matching resolution and subtype supported by source. Setting up a conversion"); 324 | if (!FindMatchingMediaType(spMediaType.put(), width, height, frameRate, GUID_NULL)) 325 | { 326 | if (!FindMatchingMediaType(spMediaType.put(), width, height, 0, GUID_NULL)) 327 | { 328 | bStatus = false; 329 | } 330 | } 331 | if (bStatus) 332 | { 333 | DWORD flags; 334 | check_hresult(spSourceReader.as()->SetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, spMediaType.get(), &flags)); 335 | check_hresult(spMediaType->SetGUID(MF_MT_SUBTYPE, preferredVideoSubType)); 336 | check_hresult(spSourceReader->SetCurrentMediaType((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, nullptr, spMediaType.get())); 337 | m_u32Width = width; 338 | m_u32Height = height; 339 | } 340 | } 341 | else 342 | { 343 | bStatus = false; 344 | } 345 | } 346 | else 347 | { 348 | DWORD flags = 0; 349 | GUID subtype; 350 | HRESULT hr = spSourceReader.as()->SetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, spMediaType.get(), &flags); 351 | 352 | if (SUCCEEDED(hr)) 353 | { 354 | _INFO("Native res set"); 355 | m_u32Width = width; 356 | m_u32Height = height; 357 | } 358 | else 359 | { 360 | _WARN("Couldn't set native res: %x", hr); 361 | } 362 | } 363 | eventCompletion.notify_one(); 364 | }; 365 | if (m_bStreamingStarted) 366 | { 367 | EnterCriticalSection(&m_critsec); 368 | // post a delegate to be processed by the sample processing thread, so the order of mediatype change is maintained 369 | m_configEventTokenList.Append(m_configEvent.add(configHandler)); 370 | LeaveCriticalSection(&m_critsec); 371 | check_hresult(spSourceReader->Flush(MF_SOURCE_READER_FIRST_VIDEO_STREAM)); 372 | eventCompletion.wait(ul); 373 | check_hresult(spSourceReader->ReadSample(MF_SOURCE_READER_FIRST_VIDEO_STREAM, m_u32SourceReaderFlags, NULL, NULL, NULL, NULL)); 374 | } 375 | else 376 | { 377 | configHandler(); 378 | } 379 | return bStatus; 380 | } 381 | 382 | WindowsMFCapture* WindowsMFCapture::CreateInstance(bool isDevice, const winrt::hstring& link, bool isController /*= true*/) 383 | { 384 | auto wr = new WindowsMFCapture(isDevice, link, isController); 385 | return wr; 386 | } 387 | 388 | } --------------------------------------------------------------------------------