机器人操作系统ROS_典型功能代码详解(6)

}

// Draw an example circle on the video stream

if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60) cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0)); //利用OpenCV的函数进行图像处理

//来在图像数据上画一个圆圈 //////cv::circle()中不是指针, 通过Mat类中指向图像数据的指针来修改图像 //cv::Mat中含有矩阵头和指向数据的指针, 应该是该指针起作用了

//再比如 Mat gray_image; cvtColor( image, gray_image, CV_BGR2GRAY ); 指针暗含在Mat类型里面了

// Update GUI Window cv::imshow(OPENCV_WINDOW, cv_ptr->image); //图像的显示//cv::imshow()针对的是Mat类型//非指针 cv::waitKey(3); // Output modified video stream image_pub_.publish(cv_ptr->toImageMsg());//发布一个sensor_msgs::ImagePtr 形式的消息 //非指针 } };

int main(int argc, char** argv) { ros::init(argc, argv, \); ImageConverter ic; ros::spin();//是小写的s return 0; }

//hud_gui_simplified.cpp

//Bults给的程序// Use the image_transport classes instead. #include

#include #include #include

#include

#include //是不是opencv2啊?????? #include

#include \

cv::Mat image_received(480, 1280, CV_8UC3, cv::Scalar::all(20)); // 只创建信息头部分

void imageCallback(const sensor_msgs::ImageConstPtr& msg) {

cv_bridge::CvImagePtr input_bridge; try {

input_bridge = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); image_received = input_bridge->image; //左右两边都不是指针量 // ROS_INFO(\ }

catch (cv_bridge::Exception& ex){

ROS_ERROR(\); return; } }

26

int main(int argc, char** argv) {

ros::init(argc, argv, \); ros::NodeHandle nh;

image_transport::ImageTransport it(nh);

image_transport::Subscriber sub = it.subscribe(\, 1, &imageCallback, ros::VoidPtr(), image_transport::TransportHints(\));

ros::Rate r(30); while(ros::ok()) {

ros::spinOnce(); r.sleep(); }

sub.shutdown();

return 0; }

//usb_cam.h

/********************************************************************* * Software License Agreement (BSD License) * Copyright (c) 2009, Robert Bosch LLC. * All rights reserved.

*********************************************************************/ #ifndef USB_CAM_USB_CAM_H #define USB_CAM_USB_CAM_H

#include //均为系统文件 #include

typedef struct { int width; int height; int bytes_per_pixel; int image_size; char *image; //指向图像数据??? int is_new;

} usb_cam_camera_image_t;

typedef enum { //一共有三种视频采集方式 : 推荐的顺序是 userptr 、 mmap 、 read-write IO_METHOD_READ, //使用read、write方式://直接使用 read 和 write 函数进行读写。这种方式最简单，但是这种方式

会在 用户空间和内核空间不断拷贝数据 ，同时在用户空间和内核空间占用了大量内存，效率不高 IO_METHOD_MMAP, //内存映射方式: //把设备里的内存映射到应用程序中的内存控件，直接处理设备内存，这是一种

有效的方式 //buffers相当于是在内核空间中分配的，这些buffer是不能被交换到虚拟内存中，一直占用着内核空间中的内存 IO_METHOD_USERPTR, //用户指针模式: 内存由用户空间的应用程序分配，并把地址传递到内核中的驱动程序，然后

由 v4l2 驱动程序直接将数据填充到用户空间的内存中 } usb_cam_io_method;

typedef enum { PIXEL_FORMAT_YUYV, PIXEL_FORMAT_UYVY, PIXEL_FORMAT_MJPEG, } usb_cam_pixel_format;

// start camera

usb_cam_camera_image_t *usb_cam_camera_start(const char* dev, usb_cam_io_method io, usb_cam_pixel_format pf, int

27

image_width, int image_height, int framerate);

// const char* dev : 只是设备名的字符串:\// shutdown camera

void usb_cam_camera_shutdown(void); // grabs a new image from the camera

void usb_cam_camera_grab_image(usb_cam_camera_image_t *image); //获取图像函数?? 从图像流中抓取一帧图像??? // enables/disable auto focus

void usb_cam_camera_set_auto_focus(int value);

#endif

//usb_cam.cpp

http://docs.ros.org/hydro/api/usb_cam/html/usb__cam_8cpp.html

/********************************************************************* * Software License Agreement (BSD License) ** Copyright (c) 2009, Robert Bosch LLC. * All rights reserved.

**********************************************************************/ #define __STDC_CONSTANT_MACROS #include #include #include #include

#include /* low-level i/o */ #include #include #include #include #include #include #include

#include // ioctl是设备驱动程序中对设备的I/O通道进行管理的函数

#include /* for videodev2.h */

extern \ {

#include //见下方的Linux系统程序文件 #include #include }

#include

#include //包中的头文件, 不用 “”???

#define CLEAR(x) memset (&(x), 0, sizeof (x)) //void *memset(void *s,int c,size_t n)作用:将已开辟内存空间s的首n个字节的值

设为值c

struct buffer { void * start; size_t length;// size_t在C语言中就有了。它是一种“整型”类型,里面保存的是一个整数,就像int, long那样。这种整数用来记

录一个大小(size)。size_t的全称应该是size type };

static char *camera_dev;

static unsigned int pixelformat;

static usb_cam_io_method io = IO_METHOD_MMAP; static int fd = -1;

struct buffer * buffers = NULL;

28

static unsigned int n_buffers = 0;

static AVFrame *avframe_camera = NULL; //AVFrame, 存储mjpeg解码后的原始数据 static AVFrame *avframe_rgb = NULL; //AVFrame, 存储mjpeg转换后的rgb数据 static AVCodec *avcodec = NULL;

static AVDictionary *avoptions = NULL;

static AVCodecContext *avcodec_context = NULL; //这是一个描述编解码器上下文的数据结构，包含了众多编解码器需要的参

数信息

static int avframe_camera_size = 0; static int avframe_rgb_size = 0;

struct SwsContext *video_sws = NULL;

static void errno_exit(const char * s) { ROS_ERROR(\, s, errno, strerror(errno)); //#include exit(EXIT_FAILURE); }

static int xioctl(int fd, int request, void * arg) { int r; do r = ioctl(fd, request, arg); while (-1==r&&EINTR==errno); //#include 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, //第一个0是第128位 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,

29

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

static unsigned char CLIPVALUE(int val) { // Old method (if) /* val = val < 0 ? 0 : val; */ /* return val > 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 void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char* r, unsigned char* g,

30