Subsribe to RadarScan topic to get coordinates and use them for further calculation [closed]

edit

Hi guys,

in my project I want to get the range and bearing of two objects that are detected from a RADAR sensor. The range and object are published through a topic called "radar_scan", the message looks like this:

Float32 range
Float32 bearing

I want to write a new node in which I subscribe to that topic to get the range and bearing and use the data for further calculation in that same node. I wrote a C++ file that looks like this:

// TheArtificialLandmarkLocalizationMethod.cpp 

#include iostream
#include Math.h
#define PI 3.14159265

using namespace std;

        double d = 2;

        double x_a = 4;
        double y_a = 1;

        double x_b = 4;
        double y_b = 3;

        double alpha;

        double x;
        double y;
        double beta;


    int main()
    {


        double r_a = 3.16227766;        // range of object A, should be provided by topic radar_scan        
        double theta_a = -18.43494882;// bearing of object A, should be provided by topic radar_scan            


        double r_b = 3.16227766;        // range of object B, should be provided by topic radar_scan                
        double theta_b = 18.43494882;   // bearing of object B, should be provided by topic radar_scan              


        y = (pow(r_a,2) - pow(r_b,2)) / (2 * (y_b - y_a)) + 0.5 * (y_a + y_b);
        x = x_a - sqrt(pow(r_a, 2) - (y - pow(y_a, 2)));                            
        alpha = asin (r_b * sin((theta_b - theta_a) * PI/180)/d) * 180/PI;          
        beta = 90 + theta_a - alpha;                                            



        cout << ("X-Koordinate Radar =  ");
        cout << x << endl;

        cout << ("Y-Koordinate Radar =  ");
        cout << y << endl;

        cout << ("Winkel Radar =    ");
        cout << beta << endl;


        return 0;
    }

The C++ Code works just fine but I have to implement it in ROS. Basically I need to get the range r_a, r_b and the bearings theta_a, theta_b from the objects by subscribing to the topic radar_scan and then use it for my formulas and in the end it should give me my desired parameters.

Can anyone help me and tell me how I could approach this problem, or if this problem is even doable in ROS? I am running Ubuntu 16.04 with kinetic.

Thanks in advance!