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

用戶節點：

#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. 用戶節點中的主環路以〜700Hz運行（由在環路中發佈的「wrench_command」主題的rostopic hz以及通過ros::Time::now()的循環時間確認），因此，spinOnce以相同速率被調用。
3. 的姿態話題回調函數被調用時〜25赫茲（由「controller_pose」主題的rostopic hz證實被髮布到了回調，通過ros::Time::now()以及環路定時）
4. 我獲得相同的行爲，甚至如果我使用AsyncSpinner而不是spinOnce，雖然只能使用rostopic hz進行確認。如預期的那樣，定時產生不穩定的輸出
5. 將用戶queue_length增加到例如10將回調速率增加到〜250Hz，但是，我想保持queue_length爲1以僅獲取最近的數據。
6. Ubuntu中的系統監視器顯示CPU利用率低於50％，所以我不認爲這是cpu瓶頸問題。

Answer 1

我解決了一個類似的問題，傳輸里程數據a，而里程數據是以100Hz傳輸，但僅以25Hz接收。原來，底層的TCP套接字緩衝了數據，將4條消息集中到一個TCP數據包中以節省傳輸成本。我相信你需要在TransportHints（）中設置tcpNoDelay爲true：

node.subscribe（...，ros :: TransportHints（）。tcpNoDelay（true））;

請注意，這發生在訂閱一側。

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