receive data from 2 callbacks and calculate values from these data

asked 2021-02-06 06:05:12 -0500

Abstractly, what should happen is as follows:

I have a node N that listens to 2 topics - T1 and T2. I calculate 2 values from data received on T1 and T2 and publish 2 output topics: OUT1 and OUT2. OUT1 is calculated every cycle of ros::spinOnce(). For OUT2, I need to collect data in a buffer of 5000 values to calculate RMS values of them.

What I do is: I have boolean variables in every callback that indicate when the buffer gets filled, and when that happens, in the main loop, when both buffers are filled, I call a function that calculated the RMS values out of the buffers.

The code is below:

The callbacks. Variables c and v are counters used to locate when buffer gets full. cReady and vReady are public booleans that help to determine when the RMS calculation can start and be published.

void currentCallback(const tb_digital_twin::Current::ConstPtr& msg)
{
    inputCurrent[0] = msg->current1;
    inputCurrent[1] = msg->current2;
    inputCurrent[2] = msg->current3;


    if(c >= SIZE_A )
    {
        currentBuffer = currents;
        cReady = true;
        currents.clear();
        currents.resize(3);
        c = 0;
        printf("CURRENT IS TRUE\n");
        printf("Currents c is %d" , c);
    }

    currents.at(0).push_back(msg->current1);
    currents.at(1).push_back(msg->current2);
    currents.at(2).push_back(msg->current3);

    c++;
}
void voltageCallback(const tb_digital_twin::Voltage::ConstPtr& msg)
{
    inputVoltage[0] = msg->voltage1;
    inputVoltage[1] = msg->voltage2;
    inputVoltage[2] = msg->voltage3;
    if(v >= SIZE_A )
    {
        voltageBuffer = voltages;
        vReady = true;
        voltages.clear();
        voltages.resize(3);
        v = 0;
        printf("VOLTAGE TRUE\n");
        printf("Currents c is %d" , c);
    }

    voltages.at(0).push_back(msg->voltage1);
    voltages.at(1).push_back(msg->voltage2);
    voltages.at(2).push_back(msg->voltage3);

    v++;
}

And the main loop (powerReactive is immediate calculation of received values, powerElectrical is the RMS values from buffers of 5k):

    while(ros::ok())
{
    ros::spinOnce();
    power.calculatePowerReactive();
    if(power.vReady && power.cReady)
    {
        if(power.calculatePowerElectrical())
        {
            powerElMsg.phase1 = power.phasePowerElectrical[0];
            powerElMsg.phase2 = power.phasePowerElectrical[1];
            powerElMsg.phase3 = power.phasePowerElectrical[2];
            powerElMsg.total = power.totalPowerElectrical;
            power.clearBuffers();
            PowerElectricalPublisher.publish(powerElMsg);
            power.setCurrentReady(false);
            power.setVoltageReady(false);
            power.setCanCalculate(false);
            ROS_DEBUG("Set to False everything");
        }
    }
    powerReactMsg.phase1 = power.phasePowerReactive[0];
    powerReactMsg.phase2 = power.phasePowerReactive[1];
    powerReactMsg.phase3 = power.phasePowerReactive[2];
    powerReactMsg.total = power.totalPowerReactive;
    PowerReactivePublisher.publish(powerReactMsg);
    rate.sleep();
}
return 0;

}

The problem:

I sometimes tend to get segmentation faults, sometimes code is executed properly, sometimes it just hangs and nothing happens. Based on some observation, I found out that sometimes one of the callbacks gets filled up faster than the other one (around 3-5 messages more, although messages are published on the subscribed topics with the same rate). Based on what I was reading, callbacks are executed in one thread (as they are spinned), so I don't really get how this behaviour can occur. How to accomplish the computation of two values from buffers correctly? Should I separate threads for these? Should I perhaps stop one callback until another one gets filled?

edit retag flag offensive close merge delete