Robotics StackExchange | Archived questions

Subscriber callback missing messages

Summary: I have a node publishing messages at ~300hz, but a callback subscribing to the topic in another node only gets called at ~25hz. The spinOnce in the subscriber node is being called at ~700hz, so I don't know why it's missing messages.

Publisher node:

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Odometry.h>

...

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sim_node");
    ros::NodeHandle nh;

    ...

    // Publishers
    tf::TransformBroadcaster tfbr;
    ros::Publisher odomPub = nh.advertise<nav_msgs::Odometry>("pose",10);

   ...

    ros::Rate r(300); // loop rate
    while(ros::ok())
    {
        ...

        // Publish pose and velocity
            ...
        odomPub.publish(msg);

        ros::spinOnce();
        r.sleep();
    }

    ros::waitForShutdown();
    return 0;
}

Subscriber node:

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Odometry.h>

...

std::mutex mtx1, mtx2;

class DataHandler
{
private:
    ros::NodeHandle nh;
    ros::Publisher odomPub;
    double lastTime;
    int lastSeq;

public:
    Eigen::Vector3d x, xDot, w;
    Eigen::Vector3d xDes, xDesDot, xDesDotDot, b1Des, b1DesDot;
    Eigen::Matrix3d R;

    DataHandler()
    {
        // Initialize data
        xDes = Eigen::Vector3d(1,0,1);
        xDesDot = Eigen::Vector3d::Zero();
        xDesDotDot = Eigen::Vector3d::Zero();
        b1Des = Eigen::Vector3d(1,0,0);
        b1DesDot = Eigen::Vector3d::Zero();
        x = Eigen::Vector3d::Zero();
        xDot = Eigen::Vector3d::Zero();
        R = Eigen::Matrix3d::Identity();

        odomPub = nh.advertise<nav_msgs::Odometry>("controller_pose",10);
        trajPub = nh.advertise<asap_control::DesiredTrajectory>("controller_desTraj",10);
        lastTime = ros::Time::now().toSec();
        lastSeq = 0;
    }

    // Get current pose and velocity
    void odomCB(const nav_msgs::OdometryConstPtr& odomMsg)
    {

        mtx1.lock();
            // Get data
        double time1 = ros::Time::now().toSec();
        x << odomMsg->pose.pose.position.x, odomMsg->pose.pose.position.y, ...;
        xDot << odomMsg->twist.twist.linear.x, odomMsg->twist.twist.linear.y, ...;
        R = Eigen::Quaterniond(odomMsg->pose.pose.orientation.w, odomMsg->pose.pose.orientation.x,...;
        w << odomMsg->twist.twist.angular.x, odomMsg->twist.twist.angular.y, ...;
        double time2 = ros::Time::now().toSec();

            // Time to extract data, < 1ms
        double delTproc = time2 - time1;
        std::cout << "\n\n";
        std::cout << "proc elapsed time: " << delTproc << "\n";
        std::cout << "proc frequency: " << 1.0/delTproc << "\n";

        odomPub.publish(odomMsg); // rostopic hz says this is publishing at ~25Hz

            // Time between callback calls, ~25Hz
        double timeNow = ros::Time::now().toSec();
        double delT = timeNow - lastTime;
        lastTime = timeNow;
        std::cout << "elapsed time: " << delT << "\n";
        std::cout << "frequency: " << 1.0/delT << "\n";

            // Message sequence IDs, shows 12 msgs skipped every call
        int seqNow = odomMsg->header.seq;
        int delSeq = seqNow - lastSeq;
        lastSeq = seqNow;
        std::cout << "delta seq: " << delSeq << "\n";
        mtx1.unlock();
    }

};

...

int main(int argc, char** argv)
{
    ros::init(argc, argv, "asap_control");
    ros::NodeHandle nh;

    ...

    // Publishers
    ros::Publisher outputPub = nh.advertise<geometry_msgs::WrenchStamped>("wrench_command",10);
    ros::Publisher debugPub = nh.advertise<asap_control::ControllerSignals>("controller_debug",10);
    tf::TransformBroadcaster tfbr;

    // Subscribers
    DataHandler callbacks;
    ros::Subscriber poseSub = nh.subscribe("pose",10,&DataHandler::odomCB,&callbacks);

    // Asynchronous threads for callback handling
    //ros::AsyncSpinner spinner(2);
    //spinner.start();

    double lastTime = ros::Time::now().toSec();


    // Main loop
    ros::Rate r(700); // loop rate
    while(ros::ok())
    {
        // Data (extracted for cleanliness further down, and thread safety)
        mtx1.lock();
        Eigen::Vector3d x = callbacks.x;
        Eigen::Vector3d xDot = callbacks.xDot;
        Eigen::Matrix3d R = callbacks.R;
        Eigen::Vector3d w = callbacks.w;
        mtx1.unlock();

            ...

        // Publish
            ...
        outputPub.publish(msg);

        // Publish debug signals
        asap_control::ControllerSignals debugMsg;
        debugMsg.x[0] = x(0);
        ...
        debugPub.publish(debugMsg);

        //double timeNow = ros::Time::now().toSec();
        //double delT = timeNow - lastTime;
        //lastTime = timeNow;
        //std::cout << "\n\n";
        //std::cout << "elapsed time: " << delT << "\n";
        //std::cout << "frequency: " << 1.0/delT << "\n";

        ros::spinOnce();
        r.sleep();
    }

    ros::waitForShutdown();
    return 0;
}

Additional info:

  1. The publisher is publishing at ~300Hz (confirmed by rostopic hz of the "pose" topic)
  2. The main loop in the subscriber node is running at ~700Hz (confirmed by rostopic hz of the "wrench_command" topic being published to in the loop, as well as loop timing via ros::Time::now()), and hence, spinOnce is being called at the same rate.
  3. The callback for the pose topic is being called at ~25 Hz (confirmed by rostopic hz of the "controller_pose" topic being published to in the callback, as well as loop timing via ros::Time::now())
  4. I get the same behavior even if I use the AsyncSpinner instead of spinOnce, though can only confirm using rostopic hz. Timing produces erratic output, as expected
  5. Increasing the subscriber queuelength to e.g., 10 increases the callback rate to ~250Hz, however, I want to keep a queuelength of 1 to get only the most recent data.
  6. System monitor in Ubuntu shows less than 50% cpu utilization, so I don't think it's cpu bottleneck issue.

Asked by anuppari on 2016-11-29 15:18:45 UTC

Comments

Answers