Publishing a Float32 topic
I am using ROS kinetic on Ubuntu My code works fine, it obtains an image and gets the angle of the road that is shown in the picture. The problem is that I want to publish it in a topic ("steering"). When I run the code I obtain the angle (I check it with the ROSINFOSTREAM) however I am not able to obtain the data in the topic "steering". When I do "rostopic list" I obtain that the "steering" topic exist. However when I do "rostopic echo /steering" I obtain nothing The code is the following:
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
//#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>
#define FILA_SUP 0.7 //Fila superior a analizar, en % sobre el total
#define FILA_INF 0.80 //Fila inferior a analizar, en % sobre el total
using namespace std;
using namespace cv;
//ros::Publisher pub; // Create a publisher object.
ros::Publisher pubSteering;
int calcCdg(uchar* ptrFila, int fila, int nc, Mat& colorFrame)
{
float suma=0;
float total=0;
float cdg=0;
for (int i=0; i<nc; i++)
{
suma += i*(255-ptrFila[i]);
total += 255-ptrFila[i];
circle(colorFrame, Point(i, fila), 1, Scalar(255, 0, 0));
}
if(total>0)
cdg=suma/total;
else
cdg=-1;
return cdg;
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
std_msgs::Float32 ang_msg;
try
{
//cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
//cv::waitKey(30);
Mat colorFrame=cv_bridge::toCvShare(msg, "bgr8")->image;
Scalar color(0,0,255);
//cv::imshow("view",colorFrame);
//cv::waitKey(30);
int nf=colorFrame.rows;
int nc=colorFrame.cols;
Mat grisFrame;
cvtColor(colorFrame, grisFrame, COLOR_BGR2GRAY); //transforma a niveles de gris
Mat binFrame;
threshold(grisFrame, binFrame, 0, 255, THRESH_BINARY_INV | CV_THRESH_OTSU);
cv::imshow("bin", binFrame);
cv::waitKey(30);
uchar* ptrFila;
float cdg_sup, cdg_inf;
int filaSup= (int) (FILA_SUP * nf);
ptrFila= binFrame.ptr<uchar>(filaSup); //apunta a la filaInf
cdg_sup = calcCdg(ptrFila, filaSup, nc, colorFrame);
int filaInf= (int) (FILA_INF * nf);
ptrFila= binFrame.ptr<uchar>(filaInf); //apunta a la filaInf
cdg_inf = calcCdg(ptrFila, filaInf, nc, colorFrame);
if(cdg_sup> -1) circle(colorFrame, Point(cdg_sup, filaSup), 3, color);
if(cdg_inf> -1) circle(colorFrame, Point(cdg_inf, filaInf), 3, color);
cv::imshow("view", colorFrame);
cv::waitKey(30);
//ROS_INFO_STREAM( "Superior: " << "(fila " << filaSup << ", col " << cdg_sup << ")");
//ROS_INFO_STREAM( "Inferior: " << "(fila " << filaInf << ", col " << cdg_inf << ")" );
// imshow("Resultado", colorFrame);
float x1, x2, y1, y2, A, B, C, dist, ang;
if( cdg_sup> -1 && cdg_inf> -1 && filaSup!=filaInf )
{
//Traslada al punto central de la ltima l nea.
y2 = nf-filaSup;
y1 = nf-filaInf;
x2 = cdg_sup-nc/2.;
x1 = cdg_inf-nc/2.;
//ROS_INFO_STREAM( "(x1, y1)= ( " << x1 << " , " << y1 << " )" );
//ROS_INFO_STREAM( "(x2, y2)= ( " << x2 << " , " << y2 << " )" );
//Recta; y distancia y ngulo
A= y1-y2;
B= x2-x1;
C= x1*y2 + x1*y1 - x1*y1 - x2*y1;
dist=C/sqrt(A*A+B*B);
ang =atan( (x2-x1)/(y2-y1) );
}
else
{
dist= -1;
ang = -1;
}
ROS_INFO_STREAM("Distancia: " << dist << " Angulo: " << ang << " rad (" << ang*180./3.141592 << " grad)");
ang_msg.data = ang;
pubSteering.publish(ang_msg);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "image_listener");
ros::NodeHandle nh;
cv::namedWindow("view");
cv::startWindowThread();
cv::namedWindow("bin");
cv::startWindowThread();
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
pubSteering = nh.advertise<std_msgs::Float32>("steering",1000);
ros::spin();
cv::destroyWindow("view");
cv::destroyWindow("bin");
}
Asked by ferkus on 2020-08-18 06:12:25 UTC
Comments
Try setting the ROS_MASTER_URI, ROS_IP and ROS_HOSTNAME in the terminal you are trying to echo the message, if you haven't already.
Asked by praskot on 2020-08-23 04:49:03 UTC
Thank you, the problem was the IP address of the computer
Asked by ferkus on 2020-08-30 07:44:52 UTC