Subscribe to topic and get/save data from two messages

edit

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; 
}