Отсутствуют сообщения обратного вызова абонента 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% загрузки процессора, поэтому я не думаю, что это проблема узких мест процессора.