Ask Your Question
0

Best way to publish 3 coordinates (array, mat, vector...)?

asked 2017-03-31 08:23:54 -0500

w.fradin gravatar image

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/22672...

http://answers.ros.org/question/23402...

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-03-31 09:39:24 -0500

emef gravatar image

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

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-03-31 08:23:54 -0500

Seen: 539 times

Last updated: Mar 31 '17