ROS RViz基本學習筆記（二）

目標：將攝像頭圖像信息加入到（一）中數據中

環境：Ubuntu 16.04 ROS kinetic Opencv2.4.9

方法：

這裏需要publish的類型爲camera/image 中的類型，可以直接通過image現有 cv_bridge::CvImage（）函數實現圖像流的傳遞。

在（一）的基礎上，修改代碼如下：

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include <string>
#include <turtlesim/Pose.h>
#include <iomanip>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <cv.h>
#include <cxcore.h>

using namespace std;
using namespace cv;

double recdata[3];

void recfunc(const turtlesim::Pose&msg){

    //ROS_INFO_STREAM(std::setprecision(2)<<std::fixed<<"postion=("<<msg.x<<","<<msg.y<<")"<<"direct="<<msg.theta);
    recdata[0] = (double)msg.x;
    recdata[1] = (double)msg.y;
    recdata[2] = (double)msg.theta;
}

main(int argc, char** argv){
    ros::init(argc , argv , "mapping");
    ros::NodeHandle n;
    ros::Rate per(10);

    ros::Publisher pub = n.advertise<visualization_msgs::Marker>("mapping_marker",1000);
    ros::Subscriber sub = n.subscribe("turtle1/pose",1000,&recfunc);
    image_transport::ImageTransport myimage(n);
    image_transport::Publisher pub2 = myimage.advertise("camera/image", 1);

    //Mat image = imread("/home/jkwang/test.png",CV_LOAD_IMAGE_COLOR);
    //if(image.empty()){
    //    cout<< "no such image!"<<endl;
    //}

    IplImage* frame = NULL;
    CvCapture* capture = cvCreateCameraCapture(1);
    Mat image;
    sensor_msgs::ImagePtr image_flow;

    while(ros::ok()){

        frame = cvQueryFrame(capture);
        image = cvarrToMat(frame);
        image_flow = cv_bridge::CvImage(std_msgs::Header(),"bgr8",image).toImageMsg();
        waitKey(1);
        /*if(!image.empty()){
            sensor_msgs::ImagePtr image_flow = cv_bridge::CvImage(std_msgs::Header(),"bgr8",image).toImageMsg();
        }*/

        visualization_msgs::Marker points,lines[11],circle;
        visualization_msgs::Marker door[2];
        visualization_msgs::Marker now_point;

        points.header.frame_id = circle.header.frame_id = door[0].header.frame_id = door[1].header.frame_id = "/my_frame";
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].header.frame_id = "/my_frame";
            }
        now_point.header.frame_id = "/my_frame";

        points.header.stamp = circle.header.stamp = door[0].header.stamp = door[1].header.stamp = ros::Time::now();
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].header.stamp = ros::Time::now();
            }
        now_point.header.stamp = ros::Time::now();

        points.ns = circle.ns = door[0].ns = door[1].ns = "mapping";
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].ns = "mapping";
            }
        now_point.ns = "mapping" ;

        points.action = circle.action = door[0].action = door[1].action = visualization_msgs::Marker::ADD;
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].action = visualization_msgs::Marker::ADD;
            }
        now_point.action = visualization_msgs::Marker::ADD;

        points.id = 0;
        circle.id = 1;
        door[0].id = 2;
        door[1].id = 3;
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].id = 4+lines_num;
            }
        now_point.id = 15;

        points.type = visualization_msgs::Marker::POINTS;
        for(int lines_num = 0;lines_num < 11;lines_num++){
             lines[lines_num].type = visualization_msgs::Marker::LINE_STRIP;
            }
        circle.type = visualization_msgs::Marker::LINE_LIST;
        door[0].type = visualization_msgs::Marker::CYLINDER;
        door[1].type = visualization_msgs::Marker::CYLINDER;
        now_point.type = visualization_msgs::Marker::CYLINDER;

        /////////////////////////////////////////init of points and lines
        points.scale.x = 0.2;
        points.scale.y = 0.2;
        points.color.g = 1.0;
        points.color.a = 1.0f;

        for(int lines_num = 0;lines_num < 11;lines_num++){
            lines[lines_num].scale.x = 0.1;
            lines[lines_num].scale.y = 0.1;
            lines[lines_num].color.r = 1.0;
            lines[lines_num].color.g = 1.0;
            lines[lines_num].color.b = 1.0;
            lines[lines_num].color.a = 1.0;
            }


        circle.scale.x = 0.1;
        circle.scale.y = 0.1;
        circle.color.b = 1.0;
        circle.color.a = 1.0;

        now_point.scale.x = 0.5;
        now_point.scale.y = 0.5;
        now_point.color.g = 1.0;
        now_point.color.a = 1.0f;

        geometry_msgs::Point p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13,p14;
        p1.x = 4.5;p1.y = 6;p1.z = 0;
        p2.x = 4.5;p2.y = -6;p2.z = 0;
        p3.x = -4.5;p3.y = -6;p3.z = 0;
        p4.x = -4.5;p4.y = 6;p4.z = 0;
        p5.x = -3; p5.y = 6;p5.z = 0;
        p6.x = 3;p6.y = 6;p6.z = 0;
        p7.x = 3;p7.y = 5;p7.z = 0;
        p8.x = -3;p8.y = 5;p8.z = 0;
        p9.x = 4.5;p9.y = 0;p9.z = 0;
        p10.x = 3;p10.y = -6;p10.z = 0;
        p11.x = -3;p11.y = -6;p11.z = 0;
        p12.x = -3;p12.y = -5;p12.z = 0;
        p13.x = 3;p13.y = -5; p13.z = 0;
        p14.x = -4.5; p14.y =0;p14.z =0;

        geometry_msgs::Point thispoint;
        //將兩座標進行全映射
        thispoint.x = -(0.818*recdata[0]-4.5);
        thispoint.y = -(-1.091*recdata[1]+6);
        thispoint.z = 0;
        //now_point.points.push_back(thispoint);
        now_point.pose.orientation.x = 0.0;
        now_point.pose.orientation.y = 90.0;
        now_point.pose.orientation.z = 0.0;
        now_point.pose.position.x = thispoint.x;
        now_point.pose.position.y = thispoint.y;
        now_point.pose.position.z = -0.5;
        now_point.scale.x = 0.15;
        now_point.scale.y = 0.15;
        now_point.scale.z = 1.0;
        now_point.color.r = 1.0f;
        now_point.color.g = 1.0f;
        now_point.color.b = 0.0f;
        now_point.color.a = 1.0;

        points.points.push_back(p1);
        points.points.push_back(p2);
        points.points.push_back(p3);
        points.points.push_back(p4);
        points.points.push_back(p5);
        points.points.push_back(p6);
        points.points.push_back(p7);
        points.points.push_back(p8);
        points.points.push_back(p9);
        points.points.push_back(p10);
        points.points.push_back(p11);
        points.points.push_back(p12);
        points.points.push_back(p13);
        points.points.push_back(p14);


        lines[0].points.push_back(p1);
        lines[0].points.push_back(p2);
        lines[1].points.push_back(p2);
        lines[1].points.push_back(p3);
        lines[2].points.push_back(p3);
        lines[2].points.push_back(p4);
        lines[3].points.push_back(p4);
        lines[3].points.push_back(p1);
        lines[4].points.push_back(p9);
        lines[4].points.push_back(p14);
        lines[5].points.push_back(p5);
        lines[5].points.push_back(p8);
        lines[6].points.push_back(p8);
        lines[6].points.push_back(p7);
        lines[7].points.push_back(p7);
        lines[7].points.push_back(p6);
        lines[8].points.push_back(p10);
        lines[8].points.push_back(p13);
        lines[9].points.push_back(p13);
        lines[9].points.push_back(p12);
        lines[10].points.push_back(p12);
        lines[10].points.push_back(p11);

        for(int i=0; i<360; i++){
            geometry_msgs::Point p;

            p.x = sin(i*2*M_PI/360);
            p.y = cos(i*2*M_PI/360);
            p.z = 0;

            circle.points.push_back(p);
        }

        ////////////////////////////////////////////init of door:
        door[0].pose.position.x = 2;
        door[0].pose.position.y = 6;
        door[0].pose.position.z = -0.5;
        door[1].pose.position.x = -2;
        door[1].pose.position.y = 6;
        door[1].pose.position.z = -0.5;

        door[0].scale.x = 0.25;
        door[0].scale.y = 0.25;
        door[0].scale.z = 1.0;
        door[1].scale.x = 0.25;
        door[1].scale.y = 0.25;
        door[1].scale.z = 1.0;

        door[0].color.r = 0.0f;
        door[0].color.g = 1.0f;
        door[0].color.b = 0.0f;
        door[0].color.a = 1.0;
        door[1].color.r = 0.0f;
        door[1].color.g = 1.0f;
        door[1].color.b = 0.0f;
        door[1].color.a = 1.0;

        door[0].pose.orientation.x = 90;
        door[0].pose.orientation.y = 0.0;
        door[0].pose.orientation.z = 0.0;
        door[1].pose.orientation.x = 90;
        door[1].pose.orientation.y = 0.0;
        door[1].pose.orientation.z = 0.0;

        door[0].lifetime = ros::Duration();
        door[1].lifetime = ros::Duration();


        for(int lines_num = 0;lines_num < 11;lines_num++){
             pub.publish(lines[lines_num]);
            }
        pub.publish(points);
        pub.publish(circle);
        pub.publish(door[0]);
        pub.publish(door[1]);
        pub.publish(now_point);
        pub2.publish(image_flow);
        //cout<<"receive data = "<< recdata << endl;

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

至此可以實現圖像流的輸出：

效果圖：