Ask Your Question
0

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

asked 2019-11-06 14:27:20 -0600

alexpawo gravatar image

updated 2019-11-07 02:30:07 -0600

gvdhoorn gravatar image

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!

edit retag flag offensive close merge delete

Comments

You need to write a subscriber and access the data you need for further calculation.

Choco93 gravatar imageChoco93 ( 2019-11-07 02:47:11 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2019-11-09 11:57:51 -0600

alexpawo gravatar image

Hi guys so I edited the code, wrote a subscriber in ROS and everything works fine. Here is the code:

class Localization {

public:
    double r_a;
    double theta_a;

    // rufe Daten von rostopic radar_scan ab
    void callback(const ti_mmwave_rospkg::RadarScan::ConstPtr& msg);

};

/* Weise range und bearing aus dem rostopic radar_scan Parameter zu */

void Localization::callback(const ti_mmwave_rospkg::RadarScan::ConstPtr& msg) { r_a = msg->range; theta_a = msg->bearing;

}

/* main function mit Algorithmus und Subscriber */

int main(int argc, char **argv) {

ros::init(argc, argv, "localization_algorithm"); // initialize new topic ros::NodeHandle node; Localization localization;

ros::Subscriber sub = node.subscribe("/ti_mmwave/radar_scan", 1000, &Localization::callback, &localization); // topic to be subscribed to

ros::Rate loop_rate(1); while (ros::ok()) { ros::spinOnce(); // show range from msg RadarScan ROS_INFO("Range is = %.2f", localization.r_a);
// show bearing from msg RadarScan
ROS_INFO("Bearing is = %.2f", localization.theta_a);
loop_rate.sleep();

    /*  Testalgorithmus: Teste ob die Daten richtig übergeben werden und für eine Berechnung richtig verwendet werden
        Richtiger Algorithmus wird noch eingefügt
        Beliebige Parameter und Werte benutzen*/

    /* double a = 15;              
    //double var;  
    //var = a + localization.r;
    //cout << ("Test Radar: a + range = ");
    cout << var << endl; */

    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;
    //double r_a ;                      
    //double theta_a;
    double r_b = 3.16227766;                        
    double theta_b = 18.43494882;

    y = (pow(localization.r_a, 2) - pow(r_b, 2)) / (2 * (y_b - y_a)) + 0.5 * (y_a + y_b);
    x = x_a - sqrt(pow(localization.r_a, 2) - (y - pow(y_a, 2)));
    alpha = asin(r_b * sin((theta_b - localization.theta_a) * PI / 180) / d) * 180 / PI;
    beta = 90 + localization.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; }

My new question would be following: Rviz shows me two points, these points are my two objects (lets assume it always shows only the two points). If I do rostopic echo /ti_mmwave/radar_scan it shows me the range and bearing of object A in message 1 and the range and bearing of object B in message two. Message 3 again object A, message 4 object B and so on.

When I wrote the subscriber I only had one Object in my test area and got the data I needed to use for my calculation.

Can I subscribe to the topic and get data from object A (through the first message published) and get data from object B (through the second message published) simultaniously? Is something like this possible?

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: 2019-11-06 14:27:20 -0600

Seen: 111 times

Last updated: Nov 09