Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

include <string>

include <vector>

include <gazebo gazebo_config.h="">

include <gazebo gazebo_client.hh="">

include <gazebo msgs="" msgs.hh="">

include <gazebo transport="" transport.hh="">

include <opencv2 opencv.hpp="">

include <opencv2 core="" core.hpp="">

include <opencv2 imgproc="" imgproc.hpp="">

include <opencv2 highgui="" highgui.hpp="">

include <opencv2 features2d="" features2d.hpp="">

define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"

void cb(ConstImageStampedPtr &msg) { // std::cout << msg->image().width() << std::endl; // std::cout << msg->image().height() << std::endl; // std::cout << msg->image().pixel_format() << std::endl; // std::cout << std::endl;

int width;
int height;
char *data;

width = (int) msg->image().width();
height = (int) msg->image().height();
data = new char[msg->image().data().length() + 1];

strcpy(data, msg->image().data().c_str());
cv::Mat image(height, width, CV_8UC3, data);

cv::imshow("camera", image);
cv::waitKey(1);
delete data;

}

int main(int argc, char **argv) { gazebo::client::setup(argc, argv); gazebo::transport::NodePtr node(new gazebo::transport::Node()); gazebo::transport::SubscriberPtr sub;

// setup
node->Init();
sub = node->Subscribe(IMAGE_TOPIC, cb);

// loop
while (true) {
    gazebo::common::Time::MSleep(10);
}

// clean up
gazebo::client::shutdown();

return 0;

}

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

include <string>

include <vector>

include <gazebo gazebo_config.h="">

include <gazebo gazebo_client.hh="">

include <gazebo msgs="" msgs.hh="">

include <gazebo transport="" transport.hh="">

include <opencv2 opencv.hpp="">

include <opencv2 core="" core.hpp="">

include <opencv2 imgproc="" imgproc.hpp="">

include <opencv2 highgui="" highgui.hpp="">

include <opencv2 features2d="" features2d.hpp="">

define
#include <string>
#include <vector>

#include <gazebo/gazebo_config.h>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"

"~/quadrotor/camera/camera/link/camera/image" void cb(ConstImageStampedPtr &msg) { // std::cout << msg->image().width() << std::endl; // std::cout << msg->image().height() << std::endl; // std::cout << msg->image().pixel_format() << std::endl; // std::cout << std::endl;

std::endl;

    int width;
 int height;
 char *data;

 width = (int) msg->image().width();
 height = (int) msg->image().height();
 data = new char[msg->image().data().length() + 1];

 strcpy(data, msg->image().data().c_str());
 cv::Mat image(height, width, CV_8UC3, data);

 cv::imshow("camera", image);
 cv::waitKey(1);
 delete data;

}

} int main(int argc, char **argv) { gazebo::client::setup(argc, argv); gazebo::transport::NodePtr node(new gazebo::transport::Node()); gazebo::transport::SubscriberPtr sub;

sub;

    // setup
 node->Init();
 sub = node->Subscribe(IMAGE_TOPIC, cb);

 // loop
 while (true) {
     gazebo::common::Time::MSleep(10);
 }

 // clean up
 gazebo::client::shutdown();

 return 0;
}

}Of particular interest is the four lines here:

    char *data;
    data = new char[msg->image().data().length() + 1];

    strcpy(data, msg->image().data().c_str());
    cv::Mat image(height, width, CV_8UC3, data);

To convert the data, we first allocate memory for the data variable (+1 for null character at the end). We then copy the image message data from msg->image().data().c_str() to data. Then we use the information about the height and width of the image to form the cv::Mat. Don't forget to delete data with delete, else you're introducing memory leaks.

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

#include <string>
#include <vector>

#include <gazebo/gazebo_config.h>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"


void cb(ConstImageStampedPtr &msg)
{
    // std::cout << msg->image().width() << std::endl;
    // std::cout << msg->image().height() << std::endl;
    // std::cout << msg->image().pixel_format() << std::endl;
    // std::cout << std::endl;

    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    data = new char[msg->image().data().length() + 1];

    strcpy(data, msg->image().data().c_str());
memcpy(data, msg->image().data().c_str(), msg->iamge().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

    cv::imshow("camera", image);
    cv::waitKey(1);
    delete data;
}

int main(int argc, char **argv)
{
    gazebo::client::setup(argc, argv);
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    gazebo::transport::SubscriberPtr sub;

    // setup
    node->Init();
    sub = node->Subscribe(IMAGE_TOPIC, cb);

    // loop
    while (true) {
        gazebo::common::Time::MSleep(10);
    }

    // clean up
    gazebo::client::shutdown();

    return 0;
}

Of particular interest is the four lines here:

    char *data;
    data = new char[msg->image().data().length() + 1];

    strcpy(data, msg->image().data().c_str());
memcpy(data, msg->image().data().c_str(), msg->iamge().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

To convert the data, we first allocate memory for the data variable (+1 for null character at the end). We then copy the image message data from msg->image().data().c_str() to data. Then we use the information about the height and width of the image to form the cv::Mat. Don't forget to delete data with delete, else you're introducing memory leaks.

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

#include <string>
#include <vector>

#include <gazebo/gazebo_config.h>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"


void cb(ConstImageStampedPtr &msg)
{
    // std::cout << msg->image().width() << std::endl;
    // std::cout << msg->image().height() << std::endl;
    // std::cout << msg->image().pixel_format() << std::endl;
    // std::cout << std::endl;

    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->iamge().data().length());
msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

    cv::imshow("camera", image);
    cv::waitKey(1);
    delete data;
}

int main(int argc, char **argv)
{
    gazebo::client::setup(argc, argv);
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    gazebo::transport::SubscriberPtr sub;

    // setup
    node->Init();
    sub = node->Subscribe(IMAGE_TOPIC, cb);

    // loop
    while (true) {
        gazebo::common::Time::MSleep(10);
    }

    // clean up
    gazebo::client::shutdown();

    return 0;
}

Of particular interest is the four lines here:

    char *data;
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->iamge().data().length());
msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

To convert the data, we first allocate memory for the data variable (+1 for null character at the end). We then copy the image message data from msg->image().data().c_str() to data. Then we use the information about the height and width of the image to form the cv::Mat. Don't forget to delete data with delete, else you're introducing memory leaks.

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

#include <string>
#include <vector>

#include <gazebo/gazebo_config.h>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"


void cb(ConstImageStampedPtr &msg)
{
    // std::cout << msg->image().width() << std::endl;
    // std::cout << msg->image().height() << std::endl;
    // std::cout << msg->image().pixel_format() << std::endl;
    // std::cout << std::endl;

    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

    cv::imshow("camera", image);
    cv::waitKey(1);
    delete data;
data;  // DO NOT FORGET TO DELETE THIS, 
                        // ELSE GAZEBO WILL TAKE ALL YOUR MEMORY
}

int main(int argc, char **argv)
{
    gazebo::client::setup(argc, argv);
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    gazebo::transport::SubscriberPtr sub;

    // setup
    node->Init();
    sub = node->Subscribe(IMAGE_TOPIC, cb);

    // loop
    while (true) {
        gazebo::common::Time::MSleep(10);
    }

    // clean up
    gazebo::client::shutdown();

    return 0;
}

Of particular interest is the four lines here:

    char *data;
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

To convert the data, we first allocate memory for the data variable (+1 for null character at the end). We then copy the image message data from msg->image().data().c_str() to data. Then we use the information about the height and width of the image to form the cv::Mat. Don't forget to delete data with delete, else you're introducing memory leaks.

I finally found the solution, the following assumes your sensor in Gazebo outputs RGB data, the code to subscribe to the image topic and convert the Gazebo Image data to OpenCV cv::Mat is as follows:

#include <string>
#include <vector>

#include <gazebo/gazebo_config.h>
#include <gazebo/gazebo_client.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#define IMAGE_TOPIC "~/quadrotor/camera/camera/link/camera/image"


void cb(ConstImageStampedPtr &msg)
{
    // std::cout << msg->image().width() << std::endl;
    // std::cout << msg->image().height() << std::endl;
    // std::cout << msg->image().pixel_format() << std::endl;
    // std::cout << std::endl;

    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

    cv::imshow("camera", image);
    cv::waitKey(1);
    delete data;  // DO NOT FORGET TO DELETE THIS, 
                   // ELSE GAZEBO WILL TAKE ALL YOUR MEMORY
}

int main(int argc, char **argv)
{
    gazebo::client::setup(argc, argv);
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    gazebo::transport::SubscriberPtr sub;

    // setup
    node->Init();
    sub = node->Subscribe(IMAGE_TOPIC, cb);

    // loop
    while (true) {
        gazebo::common::Time::MSleep(10);
    }

    // clean up
    gazebo::client::shutdown();

    return 0;
}

Of particular interest is the four lines here:

    char *data;
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    cv::Mat image(height, width, CV_8UC3, data);

To convert the data, we first allocate memory for the data variable (+1 for null character at the end). We then copy the image message data from msg->image().data().c_str() to data. Then we use the information about the height and width of the image to form the cv::Mat. Don't forget to delete data with delete, else you're introducing memory leaks.