Publishing a Float32 topic

edit

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 ROS_INFO_STREAM) 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 ...