Best way to publish 3 coordinates (array, mat, vector...)?
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 :
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