ROS-абонент обратного вызова отсутствующих сообщений

Question

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

Издатель узла: узел

#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:

#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. Издатель издательским в ~ 300Hz (подтверждено 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. Увеличение длины очереди подписчика, например, 10 увеличивает скорость обратного вызова до ~ 250 Гц, однако я хочу, чтобы длина очереди была равна 1, чтобы получать только самые последние данные.
6. Системный монитор в Ubuntu показывает менее 50% использования процессора, поэтому я не думаю, что это проблема узкого места в процессоре.

Answer 1

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

node.subscribe (..., ros :: TransportHints(). TcpNoDelay (true));

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

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