0
$\begingroup$

Rosanswers logo

Hi,

Since several days, I try to publish 3 coordinates to rosserial. Currently the only way I found, it's to publish this coordinates (x,y,z) in 3 differents topics. I would like to publish 1 vector/array or matrix (x,y,z) in one topic. I tried differents methods like :

http://answers.ros.org/question/226726/push-vector-into-multiarray-message-and-publish-it/

http://answers.ros.org/question/234028/how-to-publish-a-2-dimensional-array-of-known-values/

But each time I had compilation problems. Or when there was no problem, my publisher and subscriber were not connected to each other (/rosrun rqt_graph rqt_graph).

Here's my program ;

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv/cv.h>
#include <std_msgs/Int16.h>
#include <vector>

namespace enc = sensor_msgs::image_encodings;
using namespace cv;
using namespace std;

static const char WINDOW[] = "Image Processed";

class ImageConverter
{
    ros::NodeHandle n;
    image_transport::ImageTransport it;
    image_transport::Publisher pub;
    image_transport::Subscriber sub;

    ros::Publisher x_pos = n.advertise<std_msgs::Int16>("position_x", 1000);
    ros::Publisher y_pos = n.advertise<std_msgs::Int16>("position_y", 1000);
    ros::Publisher z_pos = n.advertise<std_msgs::Int16>("position_z", 1000);
    std_msgs::Int16 pos_x;
    std_msgs::Int16 pos_y;
    std_msgs::Int16 pos_z;

public:

    ImageConverter()
        : it(n)
    {
// ...
    }

    ~ImageConverter()
    {
    cv::destroyWindow(WINDOW);
    }

    void imageCallback(const sensor_msgs::ImageConstPtr& msg)
    {
    // Initialization + OPENCV Transformation ... 

    if(contours.size() > 0){
        int radius(1);
        vector<Point2f>center( contours.size() );
        vector<float>radius_cur( contours.size() );

        // Get smallest circle enclosing contour.
        for(int i=0; i<contours.size();i++){
          // Finds the minimal enclosing of a 2D point.
          minEnclosingCircle(Mat(contours[i]),center[i], radius_cur[i]);
            if(radius_cur[i] > radius){
              radius = int(radius_cur[i]);
              x = center[i].x;
              y = center[i].y;
              target_pixel = radius_u*radius;
              // Depth of image
              z = (f_D)/target_pixel;
              // Draws a circle.
              circle(cv_ptr->image, center[i], radius, Scalar(0,0,255),2);
            }
              // 3 publications
              delta_step_x = sgn((centre_x-x))*min(abs((centre_x-x) / 50), 5);
              pos_x.data = delta_step_x;
              x_pos.publish(pos_x);

              delta_step_y = sgn((y-centre_y))*min(abs((y-centre_y) / 50), 5);
              pos_y.data = delta_step_y;
              y_pos.publish(pos_y);

              pos_z.data = z;
              z_pos.publish(pos_z);
            }
    }

    imshow(WINDOW, cv_ptr->image);
    waitKey(3);
    pub.publish(cv_ptr->toImageMsg());
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "green_detection");
    ImageConverter bjr;
    ros::spin();
    return 0;
}

If someone can explain me and give me an example of how to proceed.

Thank for your help.

William


Originally posted by w.fradin on ROS Answers with karma: 38 on 2017-03-31

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The first link you posted has a valid solution in it, if you are having compilation or connection issues, that's a separate problem. Using the std_msgs/Int16MultiArray is probably what you want


Originally posted by emef with karma: 41 on 2017-03-31

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.