Отсутствуют сообщения обратного вызова абонента ROS

Резюме: у меня есть узел, публикующий сообщения с частотой ~300 Гц, но обратный вызов, подписывающийся на тему в другом узле, вызывается только с частотой ~25 Гц. SpinOnce в узле подписчика вызывается с частотой ~700 Гц, поэтому я не знаю, почему он пропускает сообщения.

Узел издателя:

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

Абонентский узел:

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

Дополнительная информация:

  1. Издатель публикует на частоте ~300 Гц (подтверждено rostopic hz из темы "поза")
  2. Основной цикл в узле абонента работает на частоте ~700 Гц (подтверждено rostopic hz темы "wrench_command", публикуемой в цикле, а также времени цикла через ros::Time::now()), и поэтому, spinOnce вызывается с той же скоростью.
  3. Обратный вызов для темы позы вызывается с частотой ~25 Гц (подтверждено rostopic hz темы "controller_pose", публикуемой в обратном вызове, а также циклической синхронизации через ros::Time::now())
  4. Я получаю такое же поведение, даже если я использую AsyncSpinner вместо spinOnceхотя могу подтвердить только используя rostopic hz, Сроки производит ошибочный вывод, как и ожидалось
  5. Увеличение абонентской очереди queue_length, например, до 10, увеличивает частоту обратного вызова до ~250 Гц, однако я хочу оставить для queue_length значение 1, чтобы получать только самые последние данные.
  6. Системный монитор в Ubuntu показывает менее чем 50% загрузки процессора, поэтому я не думаю, что это проблема узких мест процессора.
Источник

1 ответ

Решение

Я решил аналогичную проблему, передавая данные одометрии некоторое время назад, когда данные одометрии передавались на частоте 100 Гц, но принимались только на частоте 25 Гц. Оказалось, что базовый сокет TCP буферизует данные, объединяя 4 сообщения в один пакет TCP, чтобы сэкономить на транспортных расходах. Я считаю, что вам нужно установить значение tcpNoDelay в true в TransportHints ():

node.subscribe (...., ROS::TransportHints() tcpNoDelay(истина));

Обратите внимание, что это происходит на стороне подписки.

http://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers

Источник
Другие вопросы по тегам