Ask Your Question
0

Subscribe to topic and get/save data from two messages

asked 2019-11-11 07:09:57 -0600

alexpawo gravatar image

updated 2019-11-11 07:53:31 -0600

Mehdi. gravatar image

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.

RadarScan.msg is defined as:

Header header
uint16 point_id
float32 x
float32 y
float32 z
float32 range
float32 velocity
uint16 doppler_bin
float32 bearing
float32 intensity

I wrote a subscriber that subscribes to the radar_scan topic, but it always takes the variables that are currnetly published, either from point A or B. So for only one Point there wouldn`t be a problem. But I want to get the coordinates from both points.

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?

Here is my code for the subscriber:

class Localization {

public:
    double r;
    double theta;
    void callback(const ti_mmwave_rospkg::RadarScan::ConstPtr& msg);
};

void Localization::callback(const ti_mmwave_rospkg::RadarScan::ConstPtr& msg) 
{ 
r= msg->range;  // it should be r_a = range from message one and theta_a = bearing from message one (Point_id 0)
theta = msg->bearing; //// it should be r_b = range from message one and theta_b = bearing from message one (Point_id 1)
}

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

ros::init(argc, argv, "localization_algorithm");  
ros::NodeHandle node; Localization localization;
ros::Subscriber sub = node.subscribe("/ti_mmwave/radar_scan", 1000, &Localization::callback, &localization);
ros::Rate loop_rate(1); 

while (ros::ok()) { 
ros::spinOnce(); 
ROS_INFO("Range is = %.2f", localization.r);
ROS_INFO("Bearing is = %.2f", localization.theta);
loop_rate.sleep();

 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; 
}
edit retag flag offensive close merge delete

Comments

If the id of point A is 0 and the id of B is 1, why not using a simple if else condition to store the data for the point A when point_id is 0, and the data for point B when point_id is 1 ? You wouldn't get the data simultaneously but it would be propperly stored.

Can you modify the publisher of the data ? If so, you should create a new message RadarScanArray.msg to publish informations on both points at the same time, it would have the same definition as RadarScan.msg except that the field would be arrays (except the header field)

Delb gravatar imageDelb ( 2019-11-12 03:48:26 -0600 )edit

Could you help me implementing the if-else code in my current one?

Every time I try to implement mine I get some wrong values or it doesn't work.

alexpawo gravatar imagealexpawo ( 2019-11-16 11:26:38 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-11-18 13:17:02 -0600

alexpawo gravatar image

updated 2019-11-19 02:53:04 -0600

Delb gravatar image

I solved the problem. I put the if-else condition in the void callback function. Here is the code:

void Localization::callback(const ti_mmwave_rospkg::RadarScan::ConstPtr& msg) 
{
    id = msg->point_id;

    if (id == 0){
        r_a = msg->range;
        theta_a = msg->bearing;
        ROS_INFO("Range A is = %.2f", r_a);
        ROS_INFO("Bearing A is = %.2f", theta_a);
    }else if (id == 1){
        r_b = msg->range;
        theta_b = msg->bearing;
        ROS_INFO("Range B is = %.2f", r_b);
        ROS_INFO("Bearing B is = %.2f", theta_b);
    }

}
edit flag offensive delete link more

Comments

I've reopenned your question since the policy of this site is to accept answers by clicking the check mark button next to the answer instead of closing them.

Anyway glad it worked for you ! I would just advise you if you deal with more than 2 objects that you could store all the data into arrays and make the index of those array match the ids of your objects. That would avoir the if else condition, like this (and be sure your arrays have the correct size or just use vectors to dynamically adapt it) :

void Localization::callback(const ti_mmwave_rospkg::RadarScan::ConstPtr& msg) 
{
    id = msg->point_id;
    range_array[id] = msg->range;
    bearing_array[id] = msg->bearing;
    ROS_INFO("Range %d is = %.2f", id, range_array[id]);
    ROS_INFO("Bearing %d is = %.2f", id, bearing_array[id] );
}
Delb gravatar imageDelb ( 2019-11-19 02:58:29 -0600 )edit

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-11 07:09:57 -0600

Seen: 56 times

Last updated: Nov 19