Question

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 /timmwave/radarscan 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; 
}

Asked by alexpawo on 2019-11-11 08:09:57 UTC

Answer 1

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

}

Asked by alexpawo on 2019-11-18 14:17:02 UTC

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

Asked by Delb on 2019-11-19 03:58:29 UTC

Subscribe to topic and get/save data from two messages

Comments

Answers

Comments