diff --git a/LICENSE b/LICENSE index 8928d84f..24d43da7 100644 --- a/LICENSE +++ b/LICENSE @@ -1,8 +1,7 @@ -Software License Agreement (BSD License) - -Copyright (c) 2014, Robert Bosch LLC. All rights reserved. +Software License Agreement (BSD License 2.0) + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -13,7 +12,7 @@ are met: copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. - * Neither the name of the Robert Bosch LLC. nor the names of its + * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. diff --git a/include/usb_cam/usb_cam.h b/include/usb_cam/usb_cam.hpp similarity index 81% rename from include/usb_cam/usb_cam.h rename to include/usb_cam/usb_cam.hpp index 7abf40a8..5fcf9692 100644 --- a/include/usb_cam/usb_cam.h +++ b/include/usb_cam/usb_cam.hpp @@ -27,8 +27,9 @@ // POSSIBILITY OF SUCH DAMAGE. -#ifndef USB_CAM__USB_CAM_H_ -#define USB_CAM__USB_CAM_H_ +#ifndef USB_CAM__USB_CAM_HPP_ +#define USB_CAM__USB_CAM_HPP_ +#include "usb_cam/usb_cam_utils.hpp" #include /* for videodev2.h */ @@ -58,7 +59,8 @@ namespace usb_cam { -class UsbCam { +class UsbCam +{ public: typedef enum { @@ -84,16 +86,16 @@ class UsbCam { // start camera bool start( - const std::string& dev, io_method io, pixel_format pf, + const std::string & dev, io_method io, pixel_format pf, int image_width, int image_height, int framerate); // shutdown camera bool shutdown(void); // grabs a new image from the camera // bool get_image(sensor_msgs::msg::Image:::SharedPtr image); - bool get_image(builtin_interfaces::msg::Time& stamp, - std::string& encoding, uint32_t& height, uint32_t& width, - uint32_t& step, std::vector& data); + bool get_image( + builtin_interfaces::msg::Time & stamp, std::string & encoding, + uint32_t & height, uint32_t & width, uint32_t & step, std::vector & data); void get_formats(); // std::vector& formats); @@ -101,11 +103,11 @@ class UsbCam { bool set_auto_focus(int value); // Set video device parameters - bool set_v4l_parameter(const std::string& param, int value); - bool set_v4l_parameter(const std::string& param, const std::string& value); + bool set_v4l_parameter(const std::string & param, int value); + bool set_v4l_parameter(const std::string & param, const std::string & value); - static io_method io_method_from_string(const std::string& str); - static pixel_format pixel_format_from_string(const std::string& str); + static io_method io_method_from_string(const std::string & str); + static pixel_format pixel_format_from_string(const std::string & str); bool stop_capturing(void); bool start_capturing(void); @@ -120,7 +122,7 @@ class UsbCam { int bytes_per_pixel; int image_size; builtin_interfaces::msg::Time stamp; - char *image; + char * image; int is_new; } camera_image_t; @@ -132,8 +134,8 @@ class UsbCam { int init_mjpeg_decoder(int image_width, int image_height); - bool mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels); - bool process_image(const void * src, int len, camera_image_t *dest); + bool mjpeg2rgb(char * MJPEG, int len, char * RGB, int NumPixels); + bool process_image(const void * src, int len, camera_image_t * dest); bool read_frame(); bool uninit_device(void); bool init_read(unsigned int buffer_size); @@ -154,17 +156,17 @@ class UsbCam { int fd_; buffer * buffers_; unsigned int n_buffers_; - AVFrame *avframe_camera_; - AVFrame *avframe_rgb_; - AVCodec *avcodec_; - AVDictionary *avoptions_; - AVCodecContext *avcodec_context_; + AVFrame * avframe_camera_; + AVFrame * avframe_rgb_; + AVCodec * avcodec_; + AVDictionary * avoptions_; + AVCodecContext * avcodec_context_; int avframe_camera_size_; int avframe_rgb_size_; - struct SwsContext *video_sws_; - camera_image_t *image_; + struct SwsContext * video_sws_; + camera_image_t * image_; }; } // namespace usb_cam -#endif // USB_CAM__USB_CAM_H_ +#endif // USB_CAM__USB_CAM_HPP_ diff --git a/include/usb_cam/usb_cam_node.hpp b/include/usb_cam/usb_cam_node.hpp index e62fb37b..a029b9d8 100644 --- a/include/usb_cam/usb_cam_node.hpp +++ b/include/usb_cam/usb_cam_node.hpp @@ -29,7 +29,7 @@ #ifndef USB_CAM__USB_CAM_NODE_HPP_ #define USB_CAM__USB_CAM_NODE_HPP_ -#include "usb_cam/usb_cam.h" +#include "usb_cam/usb_cam.hpp" #include diff --git a/include/usb_cam/usb_cam_utils.hpp b/include/usb_cam/usb_cam_utils.hpp new file mode 100644 index 00000000..d8d24f5f --- /dev/null +++ b/include/usb_cam/usb_cam_utils.hpp @@ -0,0 +1,367 @@ +// Copyright 2014 Robert Bosch, LLC +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Robert Bosch, LLC nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef USB_CAM__USB_CAM_UTILS_HPP_ +#define USB_CAM__USB_CAM_UTILS_HPP_ + +#include + +#include +#include + + +namespace usb_cam +{ + +void monotonicToRealTime(const timespec & monotonic_time, timespec & real_time) +{ + struct timespec real_sample1, real_sample2, monotonic_sample; + + // TODO(lucasw) Disable interrupts here? + // otherwise what if there is a delay/interruption between sampling the times? + clock_gettime(CLOCK_REALTIME, &real_sample1); + clock_gettime(CLOCK_MONOTONIC, &monotonic_sample); + clock_gettime(CLOCK_REALTIME, &real_sample2); + + timespec time_diff; + time_diff.tv_sec = real_sample2.tv_sec - monotonic_sample.tv_sec; + time_diff.tv_nsec = real_sample2.tv_nsec - monotonic_sample.tv_nsec; + + // This isn't available outside of the kernel + // real_time = timespec_add(monotonic_time, time_diff); + + const int64_t NSEC_PER_SEC = 1000000000; + real_time.tv_sec = monotonic_time.tv_sec + time_diff.tv_sec; + real_time.tv_nsec = monotonic_time.tv_nsec + time_diff.tv_nsec; + if (real_time.tv_nsec >= NSEC_PER_SEC) { + ++real_time.tv_sec; + real_time.tv_nsec -= NSEC_PER_SEC; + } else if (real_time.tv_nsec < 0) { + --real_time.tv_sec; + real_time.tv_nsec += NSEC_PER_SEC; + } +} + +static int xioctl(int fd, int request, void * arg) +{ + int r; + + do { + r = ioctl(fd, request, arg); + continue; + } while (-1 == r && EINTR == errno); + + return r; +} + +const unsigned char uchar_clipping_table[] = { + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -128 - -121 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -120 - -113 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -112 - -105 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -104 - -97 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -96 - -89 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -88 - -81 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -80 - -73 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -72 - -65 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -64 - -57 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -56 - -49 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -48 - -41 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -40 - -33 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -32 - -25 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -24 - -17 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -16 - -9 + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, // -8 - -1 + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, + 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, + 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, + 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, + 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, + 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, + 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, + 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, + 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, + 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, + 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, + 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, + 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, + 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, + 251, 252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263 + 255, 255, 255, 255, 255, 255, 255, 255, // 264-271 + 255, 255, 255, 255, 255, 255, 255, 255, // 272-279 + 255, 255, 255, 255, 255, 255, 255, 255, // 280-287 + 255, 255, 255, 255, 255, 255, 255, 255, // 288-295 + 255, 255, 255, 255, 255, 255, 255, 255, // 296-303 + 255, 255, 255, 255, 255, 255, 255, 255, // 304-311 + 255, 255, 255, 255, 255, 255, 255, 255, // 312-319 + 255, 255, 255, 255, 255, 255, 255, 255, // 320-327 + 255, 255, 255, 255, 255, 255, 255, 255, // 328-335 + 255, 255, 255, 255, 255, 255, 255, 255, // 336-343 + 255, 255, 255, 255, 255, 255, 255, 255, // 344-351 + 255, 255, 255, 255, 255, 255, 255, 255, // 352-359 + 255, 255, 255, 255, 255, 255, 255, 255, // 360-367 + 255, 255, 255, 255, 255, 255, 255, 255, // 368-375 + 255, 255, 255, 255, 255, 255, 255, 255, // 376-383 +}; +const int clipping_table_offset = 128; + +/** Clip a value to the range 0 255 ? 255 : val; */ + + // New method (array) + return uchar_clipping_table[val + clipping_table_offset]; +} + +/** + * Conversion from YUV to RGB. + * The normal conversion matrix is due to Julien (surname unknown): + * + * [ R ] [ 1.0 0.0 1.403 ] [ Y ] + * [ G ] = [ 1.0 -0.344 -0.714 ] [ U ] + * [ B ] [ 1.0 1.770 0.0 ] [ V ] + * + * and the firewire one is similar: + * + * [ R ] [ 1.0 0.0 0.700 ] [ Y ] + * [ G ] = [ 1.0 -0.198 -0.291 ] [ U ] + * [ B ] [ 1.0 1.015 0.0 ] [ V ] + * + * Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB + * do not get you back to the same RGB!) + * [ R ] [ 1.0 0.0 1.136 ] [ Y ] + * [ G ] = [ 1.0 -0.396 -0.578 ] [ U ] + * [ B ] [ 1.0 2.041 0.002 ] [ V ] + * + */ +static bool YUV2RGB( + const unsigned char y, const unsigned char u, const unsigned char v, + unsigned char * r, unsigned char * g, unsigned char * b) +{ + const int y2 = static_cast(y); + const int u2 = static_cast(u - 128); + const int v2 = static_cast(v - 128); + // std::cerr << "YUV=("<> 16); + // int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 ); + // int b2 = y2 + ( (u2*115999) >> 16); + // This is an adjusted version (UV spread out a bit) + int r2 = y2 + ((v2 * 37221) >> 15); + int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15); + int b2 = y2 + ((u2 * 66883) >> 15); + // std::cerr << " RGB=("<> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0)); + } + return true; +} + +static bool yuyv2rgb(char * YUV, char * RGB, int NumPixels) +{ + int i, j; + unsigned char y0, y1, u, v; + unsigned char r, g, b; + + for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6) { + y0 = (unsigned char)YUV[i + 0]; + u = (unsigned char)YUV[i + 1]; + y1 = (unsigned char)YUV[i + 2]; + v = (unsigned char)YUV[i + 3]; + YUV2RGB(y0, u, v, &r, &g, &b); + RGB[j + 0] = r; + RGB[j + 1] = g; + RGB[j + 2] = b; + YUV2RGB(y1, u, v, &r, &g, &b); + RGB[j + 3] = r; + RGB[j + 4] = g; + RGB[j + 5] = b; + } + return true; +} + +void rgb242rgb(char * YUV, char * RGB, int NumPixels) +{ + memcpy(RGB, YUV, NumPixels * 3); +} +} // namespace usb_cam +#endif // USB_CAM__USB_CAM_UTILS_HPP_ diff --git a/launch/demo_launch.py b/launch/demo_launch.py index 78efabbd..2c74c148 100755 --- a/launch/demo_launch.py +++ b/launch/demo_launch.py @@ -1,4 +1,35 @@ # Copyright 2018 Lucas Walter +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Lucas Walter nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + import argparse from launch import LaunchDescription diff --git a/src/usb_cam.cpp b/src/usb_cam.cpp index 713f5360..86ba19a3 100644 --- a/src/usb_cam.cpp +++ b/src/usb_cam.cpp @@ -28,7 +28,7 @@ #define __STDC_CONSTANT_MACROS -#include +#include "usb_cam/usb_cam.hpp" #include #include @@ -60,333 +60,6 @@ namespace usb_cam { -void monotonicToRealTime(const timespec & monotonic_time, timespec & real_time) -{ - struct timespec real_sample1, real_sample2, monotonic_sample; - - // TODO(lucasw) Disable interrupts here? - // otherwise what if there is a delay/interruption between sampling the times? - clock_gettime(CLOCK_REALTIME, &real_sample1); - clock_gettime(CLOCK_MONOTONIC, &monotonic_sample); - clock_gettime(CLOCK_REALTIME, &real_sample2); - - timespec time_diff; - time_diff.tv_sec = real_sample2.tv_sec - monotonic_sample.tv_sec; - time_diff.tv_nsec = real_sample2.tv_nsec - monotonic_sample.tv_nsec; - - // This isn't available outside of the kernel - // real_time = timespec_add(monotonic_time, time_diff); - - const int64_t NSEC_PER_SEC = 1000000000; - real_time.tv_sec = monotonic_time.tv_sec + time_diff.tv_sec; - real_time.tv_nsec = monotonic_time.tv_nsec + time_diff.tv_nsec; - if (real_time.tv_nsec >= NSEC_PER_SEC) { - ++real_time.tv_sec; - real_time.tv_nsec -= NSEC_PER_SEC; - } else if (real_time.tv_nsec < 0) { - --real_time.tv_sec; - real_time.tv_nsec += NSEC_PER_SEC; - } -} - -static int xioctl(int fd, int request, void * arg) -{ - int r; - - do { - r = ioctl(fd, request, arg); - continue; - } while (-1 == r && EINTR == errno); - - return r; -} - -const unsigned char uchar_clipping_table[] = { - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -128 - -121 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -120 - -113 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -112 - -105 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -104 - -97 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -96 - -89 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -88 - -81 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -80 - -73 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -72 - -65 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -64 - -57 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -56 - -49 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -48 - -41 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -40 - -33 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -32 - -25 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -24 - -17 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -16 - -9 - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, // -8 - -1 - 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, - 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, - 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, - 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, - 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, - 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, - 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, - 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, - 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, - 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, - 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, - 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, - 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, - 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, - 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, - 251, 252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263 - 255, 255, 255, 255, 255, 255, 255, 255, // 264-271 - 255, 255, 255, 255, 255, 255, 255, 255, // 272-279 - 255, 255, 255, 255, 255, 255, 255, 255, // 280-287 - 255, 255, 255, 255, 255, 255, 255, 255, // 288-295 - 255, 255, 255, 255, 255, 255, 255, 255, // 296-303 - 255, 255, 255, 255, 255, 255, 255, 255, // 304-311 - 255, 255, 255, 255, 255, 255, 255, 255, // 312-319 - 255, 255, 255, 255, 255, 255, 255, 255, // 320-327 - 255, 255, 255, 255, 255, 255, 255, 255, // 328-335 - 255, 255, 255, 255, 255, 255, 255, 255, // 336-343 - 255, 255, 255, 255, 255, 255, 255, 255, // 344-351 - 255, 255, 255, 255, 255, 255, 255, 255, // 352-359 - 255, 255, 255, 255, 255, 255, 255, 255, // 360-367 - 255, 255, 255, 255, 255, 255, 255, 255, // 368-375 - 255, 255, 255, 255, 255, 255, 255, 255, // 376-383 -}; -const int clipping_table_offset = 128; - -/** Clip a value to the range 0 255 ? 255 : val; */ - - // New method (array) - return uchar_clipping_table[val + clipping_table_offset]; -} - -/** - * Conversion from YUV to RGB. - * The normal conversion matrix is due to Julien (surname unknown): - * - * [ R ] [ 1.0 0.0 1.403 ] [ Y ] - * [ G ] = [ 1.0 -0.344 -0.714 ] [ U ] - * [ B ] [ 1.0 1.770 0.0 ] [ V ] - * - * and the firewire one is similar: - * - * [ R ] [ 1.0 0.0 0.700 ] [ Y ] - * [ G ] = [ 1.0 -0.198 -0.291 ] [ U ] - * [ B ] [ 1.0 1.015 0.0 ] [ V ] - * - * Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB - * do not get you back to the same RGB!) - * [ R ] [ 1.0 0.0 1.136 ] [ Y ] - * [ G ] = [ 1.0 -0.396 -0.578 ] [ U ] - * [ B ] [ 1.0 2.041 0.002 ] [ V ] - * - */ -static bool YUV2RGB( - const unsigned char y, const unsigned char u, const unsigned char v, - unsigned char * r, unsigned char * g, unsigned char * b) -{ - const int y2 = static_cast(y); - const int u2 = static_cast(u - 128); - const int v2 = static_cast(v - 128); - // std::cerr << "YUV=("<> 16); - // int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 ); - // int b2 = y2 + ( (u2*115999) >> 16); - // This is an adjusted version (UV spread out a bit) - int r2 = y2 + ((v2 * 37221) >> 15); - int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15); - int b2 = y2 + ((u2 * 66883) >> 15); - // std::cerr << " RGB=("<> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0)); - } - return true; -} - -static bool yuyv2rgb(char * YUV, char * RGB, int NumPixels) -{ - int i, j; - unsigned char y0, y1, u, v; - unsigned char r, g, b; - - for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6) { - y0 = (unsigned char)YUV[i + 0]; - u = (unsigned char)YUV[i + 1]; - y1 = (unsigned char)YUV[i + 2]; - v = (unsigned char)YUV[i + 3]; - YUV2RGB(y0, u, v, &r, &g, &b); - RGB[j + 0] = r; - RGB[j + 1] = g; - RGB[j + 2] = b; - YUV2RGB(y1, u, v, &r, &g, &b); - RGB[j + 3] = r; - RGB[j + 4] = g; - RGB[j + 5] = b; - } - return true; -} - -void rgb242rgb(char * YUV, char * RGB, int NumPixels) -{ - memcpy(RGB, YUV, NumPixels * 3); -} - - UsbCam::UsbCam() : io_(IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL), avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL), @@ -1249,15 +922,15 @@ bool UsbCam::get_image( void UsbCam::get_formats() // std::vector& formats) { - RCLCPP_INFO(rclcpp::get_logger("usb_cam"), "formats:"); + RCLCPP_INFO(rclcpp::get_logger("usb_cam"), "This Cameras Supported Formats:"); struct v4l2_fmtdesc fmt; fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; fmt.index = 0; for (fmt.index = 0; xioctl(fd_, VIDIOC_ENUM_FMT, &fmt) == 0; ++fmt.index) { RCLCPP_INFO_STREAM( rclcpp::get_logger("usb_cam"), - fmt.index << " " << fmt.type << " " << fmt.flags << " '" << fmt.description << - "' " << std::hex << fmt.pixelformat << std::dec); + " " << fmt.description << "[Index: " << fmt.index << ", Type: " << fmt.type << + ", Flags: " << fmt.flags << ", PixelFormat: " << std::hex << fmt.pixelformat << "]"); struct v4l2_frmsizeenum size; size.index = 0; @@ -1266,7 +939,7 @@ void UsbCam::get_formats() // std::vector& formats) for (size.index = 0; xioctl(fd_, VIDIOC_ENUM_FRAMESIZES, &size) == 0; ++size.index) { RCLCPP_INFO_STREAM( rclcpp::get_logger("usb_cam"), - size.discrete.width << " " << size.discrete.height); + " width: " << size.discrete.width << " x height: " << size.discrete.height); struct v4l2_frmivalenum interval; interval.index = 0; interval.pixel_format = size.pixel_format; @@ -1277,8 +950,8 @@ void UsbCam::get_formats() // std::vector& formats) { if (interval.type == V4L2_FRMIVAL_TYPE_DISCRETE) { RCLCPP_INFO_STREAM( - rclcpp::get_logger("usb_cam"), interval.type << " " << - interval.discrete.numerator << " / " << + rclcpp::get_logger("usb_cam"), + " " << interval.type << " " << interval.discrete.numerator << " / " << interval.discrete.denominator); } else { RCLCPP_INFO(rclcpp::get_logger("usb_cam"), "other type");