ROS 下實現相機圖像採集與圖像傳輸到服務器,socket圖傳

原創 coding思想 2020-02-21 12:25

前言

        本文介紹一種Qt下進行ROS開發的完美方案，同時給出一個使用TCPROS進行圖像傳輸的一個例子，使用的是ros-industrial的Levi-Armstrong在2015年12月開發的一個Qt插件ros_qtc_plugin，這個插件使得Qt“新建項目”和“新建文件”選項中出現ROS的相關選項，讓我們可以直接在Qt下創建、編譯、調試ROS項目，也可以直接在Qt項目中添加ROS的package、urdf、launch，感謝Levi-Armstrong。目前這個插件還在不斷完善，有問題或者其他功能建議可以在ros_qtc_plugin的項目主頁的討論區提出。

本文是用的操作系統是ubuntu kylin 14.04中文版，ROS版本是indigo，Qt版本是Qt5.5.1(Qt Creator 4.0.3)

本文地址：http://blog.csdn.net/u013453604/article/details/52186375
視頻教程：ros_qtc_plugin插件作者Levi-Armstrong錄製的插件使用教程
參考：
ROS wiki IDEs
1. Setup ROS Qt Creator Plug in
2. Setup Qt Creator for ROS
3. Debugging Catkin Workspace
4. Where to find Qt Creator Plug in Support
github ros-industrial/ros_qtc_plugin項目主頁
插件使用問題

第一部分、入門

一、開發環境準備

//安裝開發插件ros_qtc_plugin

//安裝方法見歷史博客【傳送門】

二、新建ROS功能包與節點及編譯測試

//查看我的歷史博客【傳送門】


三、demo練手

3.1圖像發佈節點

3.1.1具體代碼及解析

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "img_pub");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("camera/image",1);

  cv::Mat image = cv::imread(argv[1],CV_LOAD_IMAGE_COLOR);
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(),"bgr8",image).toImageMsg();

  ros::Rate loop_rate(5);
  while(nh.ok())
  {
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }

  ROS_INFO("Hello world!");
}

3.1.2修改cmakeList.txt文件

    添加以下代碼，然後回到工作空間，執行catkin_make

  • find_package
find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  roscpp
  rospy
  std_msgs
  image_transport
)
#find_package(OpenCV)   #這裏我註銷了好像也沒有報錯
  • 添加頭文件包含
include_directories(
# include
  ${OpenCV_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
)

  • 生成可執行文件
add_executable(img_pub src/img_pub.cpp)
  • 連接到庫文件
target_link_libraries(img_pub
    ${catkin_LIBRARIES}
  #  ${OpenCV_LIBRARIES}  #這裏我註銷了好像也沒有報錯
)

3.1.3修改package.xml文件

package.xml文件中存放的是創建功能包時候節點所依賴的其他功能包，如果創建時候沒有添加，則必須在這裏手動添加。同時，在cmakeList.txt文件中添加以下內容。關於package.xml文件的具體講解戳這裏：https://www.cnblogs.com/qixianyu/p/6669687.html。

find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  roscpp
  rospy
  std_msgs
  image_transport
)

//博主把這兩句刪了catkin_make也還是能夠通過，先不加，如果報錯再改

  <build_depend>opencv2</build_depend>
  <build_depend>image_transport</build_depend>

  <exec_depend>image_transport</exec_depend>
  <exec_depend>opencv2</exec_depend>

3.1.4編譯

點擊qtcreator左下角的小錘子，開始編譯，如果有錯誤就會在下方顯示，雙擊跳轉到出錯位置

發現是需要在cmakeList.txt文件中添加

find_package(catkin REQUIRED COMPONENTS
  image_transport
)
注意，在構建自己的功能包的時候如果依賴了其他功能包，必須在find_package中添加依賴的包名。添加這句話後重新編譯。

3.2圖像訂閱節點

3.2.1具體代碼及分析

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "img_listener");
  ros::NodeHandle nh;
  cv::namedWindow("view");
  cv::startWindowThread();
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("/test/image",1,imageCallback);
  ros::spin();
  cv::destroyWindow("view");

  return 0;
}

3.2.2修改cmakeList.txt和package.xml文件

這個文件中沒有使用其他依賴功能包，所以package.xml文件暫時不需要做其他修改

cmakeList.txt文件中需要做一些修改：添加

  • 鏈接庫文件
target_link_libraries(img_listener
    ${catkin_LIBRARIES}
  # ${OpenCV_LIBRARIES}
)
  • 生成可執行文件
add_executable(img_listener src/img_listener.cpp

3.2.3代碼編譯

查閱資料發現，可以在~/.bashrc中添加如下代碼，創建快捷方式cw,cs,cm。分別執行' command'中的命令

#ROS alais command
alias cw='cd ~/Travel_Assistance_Robot'
alias cs='cd ~/Travel_Assistance_Robot/src'
alias cm='cd ~/Travel_Assistance_Robot && catkin_make'

3.3執行

rosrun image_trans img_pub cal.png

然後可以查看話題，發現有我們自己發佈的/test/image的話題

rosrun image_trans img_listener

顯示圖像


3.4 話題關係查看與節點關閉

3.4.1節點交互

爲了查看話題之間的關係，我們可以使用rgt_graph指令查看

rosrun rqt_graph rqt_graph

3.4.2節點關閉

想要優雅的關掉節點，請使用rosnode kill命令。當然你使用ctrl+c或者直接關閉終端也可以關閉節點，但這種方法不免過於簡單粗暴，而且並未完全的關閉節點，因爲節點管理器那裏仍然有記錄


參考資料

[1]http://wiki.ros.org/image_transport/Tutorials

[2]http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

[3]https://blog.csdn.net/github_30605157/article/details/50990493

[4]https://blog.csdn.net/xiaocainiaoshangxiao/article/details/13295625

*****************************************我是萌萌噠的分割線***************************************************


第二部分、實戰

一、圖像採集節點

//2.1、啓動相機launch文件

//2.2、相機啓動launch文件閱讀，代碼閱讀，調試

//節點間功能的切換

roslaunch ueye_cam rgb8.launch

打開相機後，顯示如下


二、圖像顯示節點

   這個就是上面的話題監聽節點，定時從指定話題獲取圖像


三、圖像傳輸節點

//節點上的圖像傳輸到服務器上

        我們需要基於校園網來傳輸圖像，兩個不同的設備連接在校園網上。問題是校園網上的節點有很多，當機器人移動到不同的位置的時候，所處的節點變化，IP地址也發生變化，此時如何和服務器進行通信？校園網採用了動態IP地址分配方式DHCP。一旦連接上校園網，只要不斷開IP地址不應該不變嗎？

        TCP/IP的話，客戶端需要指定服務器的IP地址，服務器端需要知道客戶端的IP地址。然後建立三次握手連接和關閉連接（四次握手）。

校園網的的IP地址一般都是和MAC地址綁定的，就好比路由器中的靜態地址保留功能，但是也有例外，有的是動態的分配，每次登陸，都會隨機的分配。這個要看學校的實際情況，如果學校是動態分配的，那改成靜態的IP地址是沒法上網的，也有可能造成IP地址衝突。

需要解決的問題：如何獲取動態分配DHCP的地址？目前發現有這種方式：

arp協議是一個數據鏈路層協議，負責IP地址和Mac地址的轉換。

sudo apt-get arp-scan

通過硬件地址篩選

 sudo arp-scan --interface=wlan0 --localnet | grep 54:35:30:19:68:8f

en0是網卡的設備名稱，可以通過ifconfig命令獲得，有多種網卡時注意不要寫錯

當我的IP爲 inet addr:10.88.60.14時候，通過該方式只能夠查到10.88網絡上的主機列表，而看不到其他網絡上的主機列表。比如看不到10.95網絡上的主機列表。10.95.6.210。

  • 在學校申請固定IP地址【趙海武】，那電腦每次開機IP地址會發生改變嗎？

3.1、客戶端圖像發送程序

3.1.1、TX2上圖像傳輸節點，從指定話題上獲取消息，傳輸圖像

https://blog.csdn.net/hanshuning/article/details/50581725

使用裝有ROS插件的qtcrreator來調試。這裏需要先設置一下：

點擊左邊的帶三角的run,在右邊add attach to node，選擇需要調試的節點；同時，add run step，運行需要運行的節點


然後就可以happy的調試了。

F5執行

F9設置斷點

F10跳過，執行下一步指令

F11進入，具體實現調試

注意：這裏有一個小坑，就是工程代碼裏面切記不要有中文，否則調試時候會進入彙編代碼中，如下圖所示：


設置斷點後調試，直接進入了彙編代碼界面

因爲我的代碼有中文路徑：

改成全英文路徑，重新調試即可：


3.1.2、讀取本地圖片，併發送

（client）

/*socket image transfer
* 2018.5.10
* wzw
* qt head**************************/
#include <QCoreApplication>
#include <QDebug>

//socket headfile
#include <netinet/in.h>    // for sockaddr_in
#include <sys/types.h>    // for socket
#include <sys/socket.h>    // for socket
#include <stdio.h>        // for qDebug
#include <stdlib.h>        // for exit
#include <string.h>        // for bzero
#include <time.h>                //for time_t and time
#include <arpa/inet.h>
#include <unistd.h>    //close(client_socket);

//opencv headfile
#include <vector>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using  namespace cv;
using  namespace std;

#define HELLO_WORLD_SERVER_PORT   7754
#define BUFFER_SIZE 1024

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    qDebug("test ok");
    //get server ip addr
    if (argc != 2)
    {
        qDebug("Usage: %s ServerIPAddress\n",argv[0]);
        exit(1);
    }
    qDebug("ServerIPAddress %s\n",argv[1]);

    /**************
     * socket struct
     * ***********/
    //設置一個socket地址結構client_addr,代表客戶機internet地址, 端口
    struct sockaddr_in client_addr;
    bzero(&client_addr,sizeof(client_addr)); //把一段內存區的內容全部設置爲0
   // memset(&client_addr,sizeof(client_addr));
    client_addr.sin_family = AF_INET;    //internet協議族
    client_addr.sin_addr.s_addr = htons(INADDR_ANY);//INADDR_ANY表示自動獲取本機地址
    client_addr.sin_port = htons(0);    //0表示讓系統自動分配一個空閒端口

    /*****
     * socket descriptor
     * 創建用於internet的流協議(TCP)socket,用client_socket代表客戶機socket
     * ****/
    int client_socket = socket(AF_INET,SOCK_STREAM,0);
    if( client_socket < 0)
        {
            qDebug("Create Socket Failed!\n");
            exit(1);
        }

    /******
     * bind port
     * 把客戶機的socket和客戶機的socket地址結構聯繫起來,zhiding client_socket ip,point to addr
     * *******/
    if( bind(client_socket,(struct sockaddr*)&client_addr,sizeof(client_addr)))
        {
            qDebug("Client Bind Port Failed!\n");
            exit(1);
        }

    /*****
     * get server addr from argv[1],
     * set server params
     * 設置一個socket地址結構server_addr,代表服務器的internet地址, 端口
     * server_addr.sin_addr.s_addr=inet_addr(argv[1]);//same as up inet_aton
     * ****/
    //
    struct sockaddr_in server_addr;
    bzero(&server_addr,sizeof(server_addr));
    server_addr.sin_family = AF_INET;
    if(inet_aton(argv[1],&server_addr.sin_addr) == 0)
        {
            qDebug("Server IP Address Error!\n");
            exit(1);
        }
    server_addr.sin_port = htons(HELLO_WORLD_SERVER_PORT);
    socklen_t server_addr_length = sizeof(server_addr);
    /***
     * connect request,to server ip.success return 0;fail is 1
     * 向服務器發起連接,連接成功後client_socket代表了客戶機和服務器的一個socket連接
     * **/
     if(connect(client_socket,(struct sockaddr*)&server_addr, server_addr_length) < 0)
        {
            qDebug("Can Not Connect To %s!\n",argv[1]);
            exit(1);
        }
    qDebug("success connect To %s!\n",argv[1]);

    /*********************  data transfer test  ****************************/
    /*****
     * data prepare,set buffer
     * bzero == memset
     * ****/
    char buffer[BUFFER_SIZE];
    bzero(buffer,BUFFER_SIZE);
    //從服務器接收數據到buffer中
    int length = recv(client_socket,buffer,BUFFER_SIZE,0);
    if(length < 0)
        {
            qDebug("Recieve Data From Server %s Failed!\n", argv[1]);
            exit(1);
        }
    qDebug("\n%s\n",buffer);
    bzero(buffer,BUFFER_SIZE);
    // 向服務器發buffer中的數據
    bzero(buffer,BUFFER_SIZE);
    strcpy(buffer,"Hello, World! From Client\n");
    int send_flag=send(client_socket,buffer,BUFFER_SIZE,0);
    if(!send_flag)
        qDebug("send error\n");
    qDebug("send success\n");

    /********************* 向服務器發送image  ****************************/
    //1.load image,get imagesize
    Mat s_img=imread("1.jpg");
    imshow("s_img",s_img);
    vector<uchar> encode_img;
    imencode(".jpg", s_img, encode_img);//
    /****
     * test
     * ************/
    // align value
    /*
    Mat test_img(650,552,CV_8UC3);
    uchar* pxvec=test_img.ptr<uchar>(0);
    int i,j;
    for(i=0;i<s_img.rows;i++)
    {
        pxvec = test_img.ptr<uchar>(i);
        // 3 channels range,BGR
        for(j=0;j<s_img.cols*s_img.channels();j++)
        {
            pxvec[j]=244;
        }
    }

    imshow("test_img",test_img);
    imshow("s_img_encode",s_img);
*/
    //s_img-->vector
    /*
    int i,j;
    uchar* pxvec = s_img.ptr<uchar>(0);
    for(i=0;i<s_img.rows;i++)
    {
        pxvec = s_img.ptr<uchar>(i);
        // 3 channels range,BGR
        for(j=0;j<s_img.cols*s_img.channels();j++)
        {
            //qDebug("px value is %d",pxvec[j]) ;
            encode_img.push_back(pxvec[j]);
        }
    }
    */
    //get send_buffer
    int encode_img_size=encode_img.size();
    int s_img_size=s_img.rows*s_img.cols*3;
    qDebug("filesize is %d,width*hight*3 is %d\n",encode_img_size,s_img_size);

    uchar* send_buffer=new uchar[encode_img.size()];
    copy(encode_img.begin(),encode_img.end(),send_buffer);

    //2.send file_name
    int toSend=encode_img_size, receive  = 0, finished = 0;
    QString photoName;
    char* file_name;
    char char_len[10];
    photoName=QString("1.jpg");
    file_name=photoName.toLatin1().data();
    // file_name,qDebug file_name be empty
    //qDebug("file name is %s\n",file_name);
    bzero(buffer,BUFFER_SIZE);
    send_flag=send(client_socket, file_name, 10, 0);
    if(!send_flag)
      {
        qDebug(" send file_name failed\n ");
        exit(1);
      }
    qDebug("success send file_name \n");
    //3.send image length
    sprintf(char_len, "%d", toSend);
    send(client_socket, char_len, 10, 0);//hello world!!hei hei(xiao)!!  strlen(char_len)這裏要寫一個固定長度，然後讓服務器端讀出一個固定長度，否則會出錯
    qDebug("char_len is %s\n",char_len);

    // send test

    //4.send image data
    while(toSend  >  0)
    {
        int size = qMin(toSend, 1000);//以前是1000
        if((receive = send(client_socket, send_buffer + finished, size, 0)))  //send wenzi
        {
            if(receive==-1)
            {
                printf ("receive error");
                break;
            }
            else
            {
                toSend -= receive;// shengxia de unsend
                finished += receive; //sended
            }
        }
    }

    //5.close socket
    close(client_socket);
    qDebug("close socket\n");
    return a.exec();
}

（server）

#include <QCoreApplication>
//本文件是服務器的代碼
#include <netinet/in.h>    // for sockaddr_in
#include <sys/types.h>    // for socket
#include <sys/socket.h>    // for socket
#include <stdio.h>        // for QDebug
#include <stdlib.h>        // for exit
#include <string.h>        // for bzero
#include <time.h>                //for time_t and time
#include <unistd.h>
//#include <printf>

#define HELLO_WORLD_SERVER_PORT 7754
#define LENGTH_OF_LISTEN_QUEUE 20
#define BUFFER_SIZE 1024

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    ////////////////////////////////////
    //服務器代碼
    ///////////////////////////////////

    //設置一個socket地址結構server_addr,代表服務器internet地址, 端口
    struct sockaddr_in server_addr;
    bzero(&server_addr,sizeof(server_addr)); //把一段內存區的內容全部設置爲0
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = htons(INADDR_ANY);
    server_addr.sin_port = htons(HELLO_WORLD_SERVER_PORT);
    char file_name[10] = "25.jpg";
    char* file_buffer = new char[552 * 650 * 3];


    //創建用於internet的流協議(TCP)socket,用server_socket代表服務器socket
    int server_socket = socket(AF_INET,SOCK_STREAM,0);
    if( server_socket < 0)
        {
            printf("Create Socket Failed!");
            exit(1);
        }

    //把socket和socket地址結構聯繫起來
    if( bind(server_socket,(struct sockaddr*)&server_addr,sizeof(server_addr)))
        {
            printf("Server Bind Port : %d Failed!", HELLO_WORLD_SERVER_PORT);
            exit(1);
        }

        listen(server_socket, LENGTH_OF_LISTEN_QUEUE);

    while (1) //服務器端要一直運行
    {

        struct sockaddr_in client_addr;
        socklen_t length = sizeof(client_addr);
        //accept() kai shi jie shou shu ju
        int new_server_socket = accept(server_socket,(struct sockaddr*)&client_addr,&length);
        if ( new_server_socket < 0)
        {
            printf("Server Accept Failed!\n");
            break;
        }

        char buffer[BUFFER_SIZE];
        bzero(buffer, BUFFER_SIZE);

        //send hello world
        strcpy(buffer,"Hello,World! 從服務器來！");
        strcat(buffer,"\n"); //C語言字符串連接

        send(new_server_socket,buffer,BUFFER_SIZE,0);

        bzero(buffer,BUFFER_SIZE);
        //接收客戶端發送來的信息到buffer中
        length = recv(new_server_socket,buffer,BUFFER_SIZE,0);
        printf("%s\n",buffer);
        printf("regional file_name is %s\n",file_name);
        recv(new_server_socket, file_name, 10, 0);
        printf("received file_name is %s\n", file_name);

        char char_len[10];
        long file_length,read_length;
        char directory[100] = "/home/sa/abc/";
        int finished = 0;

        recv(new_server_socket, char_len, 10, 0);
        read_length = atoi(char_len);
        file_length = read_length;
        printf("received file_length is %d\n", read_length);

        while (read_length > 1000)
        {
            int receive = recv(new_server_socket, file_buffer + finished, 1000, 0);
            //strcpy(file_buff, buff);
            read_length -= receive;
            finished += receive;
        }
        read_length = recv(new_server_socket, file_buffer + finished, 1000, 0);

        FILE* fp = fopen(strcat(directory, file_name), "wb");
        if (fp == NULL)
            printf("create file failed!\n");
        else
        {

            fwrite(file_buffer, 1, file_length, fp);
            fclose(fp);
            printf("create file succed!\n");
        }

        //關閉與客戶端的連接
        close(new_server_socket);
    }
    //關閉監聽用的socket
    close(server_socket);

    return a.exec();
}

3.1.3從指定話題讀取攝像頭頭像，並按序號保存

/*****
 * sub img from img topic:/camera/image_raw
 * *****/
//qt head

/***
 * ros img headfile
 * ****/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>

using namespace std;
/****
 * socket headfile
 * ******/

unsigned int fileNum = 1;
bool imageSaveFlag;
void imageTransCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try
  {
    //imgshow
    cv::imshow("show image", cv_bridge::toCvShare(msg, "bgr8")->image);
    char key;
    key=cv::waitKey(33);
    if(key==32)
      imageSaveFlag=true;
    if(imageSaveFlag)
    {
      stringstream fileName;
      stringstream filePath;
      fileName<<"goal rgbImage"<<fileNum<<".jpg";
      filePath<<"/home/nvidia/Travel_Assistance_Robot/image/"<<fileNum<<".jpg";
      string fn=fileName.str();
      string fp=filePath.str();
      cv::imwrite(fp,cv_bridge::toCvShare(msg, "bgr8")->image);
      imageSaveFlag =false;
      fileNum ++;
      cout<<fileName<<"had saved."<<endl;
    }
    //imgwrite



   //socket image trans

  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "img_listener");
  ros::NodeHandle nh;
  cv::namedWindow("show image");
  cv::startWindowThread();
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("/camera/image_raw",1,imageTransCallback);


  ros::spin();

  cv::destroyWindow("view");

  return 0;
}

3.1.4、結合3.1.1和3.1.2，從指定話題獲取圖像，並使用socket將圖像傳輸至服務器

//github 

https://github.com/zuwuwang/qt_ros_ws/blob/master/src/guitest/src/socketsend_node.cpp
https://github.com/zuwuwang/qt_ros_ws/blob/master/src/guitest/include/guitest/socketsend_node.hpp

//將獲話題消息放到回調函數中

//ROS下定時從節點獲取圖像

//將socket節點放到回調函數中

vim 查看圖像的二進制數據

Vim 可以用來查看和編輯二進制文件
vim -b fileName   加上-b參數，以二進制打開
然後輸入命令  :%!xxd -g 1  切換到十六進制模式顯示,:wq退出


三、服務器異常檢測結果回傳

        客戶端C++代碼發送圖像，服務器端用python實現了接收圖像，連續接收。然後接收到圖像之後，輸入到MCNN進行檢測，檢測結果回傳。

// 桑永龍，發送檢測結果

參考資料

[1]https://blog.csdn.net/zhuoyueljl/article/details/53557822

[2] 

coding思想
發佈了49 篇原創文章 · 獲贊 97 · 訪問量 24萬+
私信 關注
發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章