|
| 1 | +/**************************************************************************/ |
| 2 | +/* camera_feed_linux.cpp */ |
| 3 | +/**************************************************************************/ |
| 4 | +/* This file is part of: */ |
| 5 | +/* GODOT ENGINE */ |
| 6 | +/* https://godotengine.org */ |
| 7 | +/**************************************************************************/ |
| 8 | +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ |
| 9 | +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ |
| 10 | +/* */ |
| 11 | +/* Permission is hereby granted, free of charge, to any person obtaining */ |
| 12 | +/* a copy of this software and associated documentation files (the */ |
| 13 | +/* "Software"), to deal in the Software without restriction, including */ |
| 14 | +/* without limitation the rights to use, copy, modify, merge, publish, */ |
| 15 | +/* distribute, sublicense, and/or sell copies of the Software, and to */ |
| 16 | +/* permit persons to whom the Software is furnished to do so, subject to */ |
| 17 | +/* the following conditions: */ |
| 18 | +/* */ |
| 19 | +/* The above copyright notice and this permission notice shall be */ |
| 20 | +/* included in all copies or substantial portions of the Software. */ |
| 21 | +/* */ |
| 22 | +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ |
| 23 | +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ |
| 24 | +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ |
| 25 | +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ |
| 26 | +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ |
| 27 | +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ |
| 28 | +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ |
| 29 | +/**************************************************************************/ |
| 30 | + |
| 31 | +#include "camera_feed_linux.h" |
| 32 | + |
| 33 | +#include <fcntl.h> |
| 34 | +#include <sys/ioctl.h> |
| 35 | +#include <sys/mman.h> |
| 36 | +#include <unistd.h> |
| 37 | + |
| 38 | +void CameraFeedLinux::update_buffer_thread_func(void *p_func) { |
| 39 | + if (p_func) { |
| 40 | + CameraFeedLinux *camera_feed_linux = (CameraFeedLinux *)p_func; |
| 41 | + camera_feed_linux->_update_buffer(); |
| 42 | + } |
| 43 | +} |
| 44 | + |
| 45 | +void CameraFeedLinux::_update_buffer() { |
| 46 | + while (!exit_flag.is_set()) { |
| 47 | + _read_frame(); |
| 48 | + usleep(10000); |
| 49 | + } |
| 50 | +} |
| 51 | + |
| 52 | +void CameraFeedLinux::_query_device(const String &p_device_name) { |
| 53 | + file_descriptor = open(p_device_name.ascii(), O_RDWR | O_NONBLOCK, 0); |
| 54 | + ERR_FAIL_COND_MSG(file_descriptor == -1, vformat("Cannot open file descriptor for %s. Error: %d.", p_device_name, errno)); |
| 55 | + |
| 56 | + struct v4l2_capability capability; |
| 57 | + if (ioctl(file_descriptor, VIDIOC_QUERYCAP, &capability) == -1) { |
| 58 | + ERR_FAIL_MSG(vformat("Cannot query device. Error: %d.", errno)); |
| 59 | + } |
| 60 | + name = String((char *)capability.card); |
| 61 | + |
| 62 | + for (int index = 0;; index++) { |
| 63 | + struct v4l2_fmtdesc fmtdesc; |
| 64 | + memset(&fmtdesc, 0, sizeof(fmtdesc)); |
| 65 | + fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 66 | + fmtdesc.index = index; |
| 67 | + |
| 68 | + if (ioctl(file_descriptor, VIDIOC_ENUM_FMT, &fmtdesc) == -1) { |
| 69 | + break; |
| 70 | + } |
| 71 | + |
| 72 | + for (int res_index = 0;; res_index++) { |
| 73 | + struct v4l2_frmsizeenum frmsizeenum; |
| 74 | + memset(&frmsizeenum, 0, sizeof(frmsizeenum)); |
| 75 | + frmsizeenum.pixel_format = fmtdesc.pixelformat; |
| 76 | + frmsizeenum.index = res_index; |
| 77 | + |
| 78 | + if (ioctl(file_descriptor, VIDIOC_ENUM_FRAMESIZES, &frmsizeenum) == -1) { |
| 79 | + break; |
| 80 | + } |
| 81 | + |
| 82 | + for (int framerate_index = 0;; framerate_index++) { |
| 83 | + struct v4l2_frmivalenum frmivalenum; |
| 84 | + memset(&frmivalenum, 0, sizeof(frmivalenum)); |
| 85 | + frmivalenum.pixel_format = fmtdesc.pixelformat; |
| 86 | + frmivalenum.width = frmsizeenum.discrete.width; |
| 87 | + frmivalenum.height = frmsizeenum.discrete.height; |
| 88 | + frmivalenum.index = framerate_index; |
| 89 | + |
| 90 | + if (ioctl(file_descriptor, VIDIOC_ENUM_FRAMEINTERVALS, &frmivalenum) == -1) { |
| 91 | + if (framerate_index == 0) { |
| 92 | + _add_format(fmtdesc, frmsizeenum.discrete, -1, 1); |
| 93 | + } |
| 94 | + break; |
| 95 | + } |
| 96 | + |
| 97 | + _add_format(fmtdesc, frmsizeenum.discrete, frmivalenum.discrete.numerator, frmivalenum.discrete.denominator); |
| 98 | + } |
| 99 | + } |
| 100 | + } |
| 101 | + |
| 102 | + close(file_descriptor); |
| 103 | +} |
| 104 | + |
| 105 | +void CameraFeedLinux::_add_format(v4l2_fmtdesc p_description, v4l2_frmsize_discrete p_size, int p_frame_numerator, int p_frame_denominator) { |
| 106 | + FeedFormat feed_format; |
| 107 | + feed_format.width = p_size.width; |
| 108 | + feed_format.height = p_size.height; |
| 109 | + feed_format.format = String((char *)p_description.description); |
| 110 | + feed_format.frame_numerator = p_frame_numerator; |
| 111 | + feed_format.frame_denominator = p_frame_denominator; |
| 112 | + feed_format.pixel_format = p_description.pixelformat; |
| 113 | + print_verbose(vformat("%s %dx%d@%d/%dfps", (char *)p_description.description, p_size.width, p_size.height, p_frame_denominator, p_frame_numerator)); |
| 114 | + formats.push_back(feed_format); |
| 115 | +} |
| 116 | + |
| 117 | +bool CameraFeedLinux::_request_buffers() { |
| 118 | + struct v4l2_requestbuffers requestbuffers; |
| 119 | + |
| 120 | + memset(&requestbuffers, 0, sizeof(requestbuffers)); |
| 121 | + requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 122 | + requestbuffers.memory = V4L2_MEMORY_MMAP; |
| 123 | + requestbuffers.count = 4; |
| 124 | + |
| 125 | + if (ioctl(file_descriptor, VIDIOC_REQBUFS, &requestbuffers) == -1) { |
| 126 | + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_REQBUFS) error: %d.", errno)); |
| 127 | + } |
| 128 | + |
| 129 | + ERR_FAIL_COND_V_MSG(requestbuffers.count < 2, false, "Not enough buffers granted."); |
| 130 | + |
| 131 | + buffer_count = requestbuffers.count; |
| 132 | + buffers = new StreamingBuffer[buffer_count]; |
| 133 | + |
| 134 | + for (unsigned int i = 0; i < buffer_count; i++) { |
| 135 | + struct v4l2_buffer buffer; |
| 136 | + |
| 137 | + memset(&buffer, 0, sizeof(buffer)); |
| 138 | + buffer.type = requestbuffers.type; |
| 139 | + buffer.memory = V4L2_MEMORY_MMAP; |
| 140 | + buffer.index = i; |
| 141 | + |
| 142 | + if (ioctl(file_descriptor, VIDIOC_QUERYBUF, &buffer) == -1) { |
| 143 | + delete[] buffers; |
| 144 | + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_QUERYBUF) error: %d.", errno)); |
| 145 | + } |
| 146 | + |
| 147 | + buffers[i].length = buffer.length; |
| 148 | + buffers[i].start = mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, file_descriptor, buffer.m.offset); |
| 149 | + |
| 150 | + if (buffers[i].start == MAP_FAILED) { |
| 151 | + for (unsigned int b = 0; b < i; b++) { |
| 152 | + _unmap_buffers(i); |
| 153 | + } |
| 154 | + delete[] buffers; |
| 155 | + ERR_FAIL_V_MSG(false, "Mapping buffers failed."); |
| 156 | + } |
| 157 | + } |
| 158 | + |
| 159 | + return true; |
| 160 | +} |
| 161 | + |
| 162 | +bool CameraFeedLinux::_start_capturing() { |
| 163 | + for (unsigned int i = 0; i < buffer_count; i++) { |
| 164 | + struct v4l2_buffer buffer; |
| 165 | + |
| 166 | + memset(&buffer, 0, sizeof(buffer)); |
| 167 | + buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 168 | + buffer.memory = V4L2_MEMORY_MMAP; |
| 169 | + buffer.index = i; |
| 170 | + |
| 171 | + if (ioctl(file_descriptor, VIDIOC_QBUF, &buffer) == -1) { |
| 172 | + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_QBUF) error: %d.", errno)); |
| 173 | + } |
| 174 | + } |
| 175 | + |
| 176 | + enum v4l2_buf_type type; |
| 177 | + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 178 | + |
| 179 | + if (ioctl(file_descriptor, VIDIOC_STREAMON, &type) == -1) { |
| 180 | + ERR_FAIL_V_MSG(false, vformat("ioctl(VIDIOC_STREAMON) error: %d.", errno)); |
| 181 | + } |
| 182 | + |
| 183 | + return true; |
| 184 | +} |
| 185 | + |
| 186 | +void CameraFeedLinux::_read_frame() { |
| 187 | + struct v4l2_buffer buffer; |
| 188 | + memset(&buffer, 0, sizeof(buffer)); |
| 189 | + buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 190 | + buffer.memory = V4L2_MEMORY_MMAP; |
| 191 | + |
| 192 | + if (ioctl(file_descriptor, VIDIOC_DQBUF, &buffer) == -1) { |
| 193 | + if (errno != EAGAIN) { |
| 194 | + print_error(vformat("ioctl(VIDIOC_DQBUF) error: %d.", errno)); |
| 195 | + exit_flag.set(); |
| 196 | + } |
| 197 | + return; |
| 198 | + } |
| 199 | + |
| 200 | + buffer_decoder->decode(buffers[buffer.index]); |
| 201 | + |
| 202 | + if (ioctl(file_descriptor, VIDIOC_QBUF, &buffer) == -1) { |
| 203 | + print_error(vformat("ioctl(VIDIOC_QBUF) error: %d.", errno)); |
| 204 | + } |
| 205 | + |
| 206 | + emit_signal(SNAME("frame_changed")); |
| 207 | +} |
| 208 | + |
| 209 | +void CameraFeedLinux::_stop_capturing() { |
| 210 | + enum v4l2_buf_type type; |
| 211 | + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 212 | + |
| 213 | + if (ioctl(file_descriptor, VIDIOC_STREAMOFF, &type) == -1) { |
| 214 | + print_error(vformat("ioctl(VIDIOC_STREAMOFF) error: %d.", errno)); |
| 215 | + } |
| 216 | +} |
| 217 | + |
| 218 | +void CameraFeedLinux::_unmap_buffers(unsigned int p_count) { |
| 219 | + for (unsigned int i = 0; i < p_count; i++) { |
| 220 | + munmap(buffers[i].start, buffers[i].length); |
| 221 | + } |
| 222 | +} |
| 223 | + |
| 224 | +void CameraFeedLinux::_start_thread() { |
| 225 | + exit_flag.clear(); |
| 226 | + thread = memnew(Thread); |
| 227 | + thread->start(CameraFeedLinux::update_buffer_thread_func, this); |
| 228 | +} |
| 229 | + |
| 230 | +String CameraFeedLinux::get_device_name() const { |
| 231 | + return device_name; |
| 232 | +} |
| 233 | + |
| 234 | +bool CameraFeedLinux::activate_feed() { |
| 235 | + file_descriptor = open(device_name.ascii(), O_RDWR | O_NONBLOCK, 0); |
| 236 | + if (_request_buffers() && _start_capturing()) { |
| 237 | + buffer_decoder = _create_buffer_decoder(); |
| 238 | + _start_thread(); |
| 239 | + return true; |
| 240 | + } |
| 241 | + ERR_FAIL_V_MSG(false, "Could not activate feed."); |
| 242 | +} |
| 243 | + |
| 244 | +BufferDecoder *CameraFeedLinux::_create_buffer_decoder() { |
| 245 | + switch (formats[selected_format].pixel_format) { |
| 246 | + case V4L2_PIX_FMT_MJPEG: |
| 247 | + case V4L2_PIX_FMT_JPEG: |
| 248 | + return memnew(JpegBufferDecoder(this)); |
| 249 | + case V4L2_PIX_FMT_YUYV: |
| 250 | + case V4L2_PIX_FMT_YYUV: |
| 251 | + case V4L2_PIX_FMT_YVYU: |
| 252 | + case V4L2_PIX_FMT_UYVY: |
| 253 | + case V4L2_PIX_FMT_VYUY: { |
| 254 | + String output = parameters["output"]; |
| 255 | + if (output == "separate") { |
| 256 | + return memnew(SeparateYuyvBufferDecoder(this)); |
| 257 | + } |
| 258 | + if (output == "grayscale") { |
| 259 | + return memnew(YuyvToGrayscaleBufferDecoder(this)); |
| 260 | + } |
| 261 | + if (output == "copy") { |
| 262 | + return memnew(CopyBufferDecoder(this, false)); |
| 263 | + } |
| 264 | + return memnew(YuyvToRgbBufferDecoder(this)); |
| 265 | + } |
| 266 | + default: |
| 267 | + return memnew(CopyBufferDecoder(this, true)); |
| 268 | + } |
| 269 | +} |
| 270 | + |
| 271 | +void CameraFeedLinux::deactivate_feed() { |
| 272 | + exit_flag.set(); |
| 273 | + thread->wait_to_finish(); |
| 274 | + memdelete(thread); |
| 275 | + _stop_capturing(); |
| 276 | + _unmap_buffers(buffer_count); |
| 277 | + delete[] buffers; |
| 278 | + memdelete(buffer_decoder); |
| 279 | + for (int i = 0; i < CameraServer::FEED_IMAGES; i++) { |
| 280 | + RID placeholder = RenderingServer::get_singleton()->texture_2d_placeholder_create(); |
| 281 | + RenderingServer::get_singleton()->texture_replace(texture[i], placeholder); |
| 282 | + } |
| 283 | + base_width = 0; |
| 284 | + base_height = 0; |
| 285 | + close(file_descriptor); |
| 286 | + |
| 287 | + emit_signal(SNAME("format_changed")); |
| 288 | +} |
| 289 | + |
| 290 | +Array CameraFeedLinux::get_formats() const { |
| 291 | + Array result; |
| 292 | + for (const FeedFormat &format : formats) { |
| 293 | + Dictionary dictionary; |
| 294 | + dictionary["width"] = format.width; |
| 295 | + dictionary["height"] = format.height; |
| 296 | + dictionary["format"] = format.format; |
| 297 | + dictionary["frame_numerator"] = format.frame_numerator; |
| 298 | + dictionary["frame_denominator"] = format.frame_denominator; |
| 299 | + result.push_back(dictionary); |
| 300 | + } |
| 301 | + return result; |
| 302 | +} |
| 303 | + |
| 304 | +CameraFeed::FeedFormat CameraFeedLinux::get_format() const { |
| 305 | + return formats[selected_format]; |
| 306 | +} |
| 307 | + |
| 308 | +bool CameraFeedLinux::set_format(int p_index, const Dictionary &p_parameters) { |
| 309 | + ERR_FAIL_COND_V_MSG(active, false, "Feed is active."); |
| 310 | + ERR_FAIL_INDEX_V_MSG(p_index, formats.size(), false, "Invalid format index."); |
| 311 | + |
| 312 | + parameters = p_parameters.duplicate(); |
| 313 | + selected_format = p_index; |
| 314 | + |
| 315 | + FeedFormat feed_format = formats[p_index]; |
| 316 | + |
| 317 | + file_descriptor = open(device_name.ascii(), O_RDWR | O_NONBLOCK, 0); |
| 318 | + |
| 319 | + struct v4l2_format format; |
| 320 | + memset(&format, 0, sizeof(format)); |
| 321 | + format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 322 | + format.fmt.pix.width = feed_format.width; |
| 323 | + format.fmt.pix.height = feed_format.height; |
| 324 | + format.fmt.pix.pixelformat = feed_format.pixel_format; |
| 325 | + |
| 326 | + if (ioctl(file_descriptor, VIDIOC_S_FMT, &format) == -1) { |
| 327 | + close(file_descriptor); |
| 328 | + ERR_FAIL_V_MSG(false, vformat("Cannot set format, error: %d.", errno)); |
| 329 | + } |
| 330 | + |
| 331 | + if (feed_format.frame_numerator > 0) { |
| 332 | + struct v4l2_streamparm param; |
| 333 | + memset(¶m, 0, sizeof(param)); |
| 334 | + |
| 335 | + param.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| 336 | + param.parm.capture.capability = V4L2_CAP_TIMEPERFRAME; |
| 337 | + param.parm.capture.timeperframe.numerator = feed_format.frame_numerator; |
| 338 | + param.parm.capture.timeperframe.denominator = feed_format.frame_denominator; |
| 339 | + |
| 340 | + if (ioctl(file_descriptor, VIDIOC_S_PARM, ¶m) == -1) { |
| 341 | + close(file_descriptor); |
| 342 | + ERR_FAIL_V_MSG(false, vformat("Cannot set framerate, error: %d.", errno)); |
| 343 | + } |
| 344 | + } |
| 345 | + close(file_descriptor); |
| 346 | + |
| 347 | + emit_signal(SNAME("format_changed")); |
| 348 | + |
| 349 | + return true; |
| 350 | +} |
| 351 | + |
| 352 | +CameraFeedLinux::CameraFeedLinux(const String &p_device_name) : |
| 353 | + CameraFeed() { |
| 354 | + device_name = p_device_name; |
| 355 | + _query_device(device_name); |
| 356 | + set_format(0, Dictionary()); |
| 357 | +} |
| 358 | + |
| 359 | +CameraFeedLinux::~CameraFeedLinux() { |
| 360 | + if (is_active()) { |
| 361 | + deactivate_feed(); |
| 362 | + } |
| 363 | +} |
0 commit comments