ROS订阅者回调缺少消息
ROS subscriber callback missing messages
摘要:我有一个节点在 ~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 << "nn";
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<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
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 << "nn";
//std::cout << "elapsed time: " << delT << "n";
//std::cout << "frequency: " << 1.0/delT << "n";
ros::spinOnce();
r.sleep();
}
ros::waitForShutdown();
return 0;
}
附加信息:
- 出版商以~300Hz发布(由"姿势"主题
rostopic hz
确认) - 订阅者节点中的主循环以 ~700Hz 运行(通过循环中发布到的"wrench_command"主题的
rostopic hz
以及通过ros::Time::now()
的循环计时确认),因此,spinOnce
以相同的速率被调用。
姿势主题的回调以 - ~25 Hz 调用(通过回调中发布到的"controller_pose"主题的
rostopic hz
以及通过ros::Time::now()
的循环计时确认) - 即使我使用
AsyncSpinner
而不是spinOnce
,我也得到相同的行为,尽管只能确认使用rostopic hz
.时序产生不稳定的输出,如预期的那样 - 将订阅者queue_length增加到例如 10 会将回调速率增加到 ~250Hz,但是,我想保持 1 queue_length以仅获取最新数据。
- Ubuntu 中的系统监视器显示 CPU 利用率低于 50%,因此我认为这不是 CPU 瓶颈问题。
不久前,我解决了传输测程数据的类似问题,其中测程数据以 100Hz 传输,但仅以 25 Hz 接收。 事实证明,底层 TCP 套接字正在缓冲数据,将 4 条消息集中到单个 TCP 数据包中以节省传输成本。我相信你需要在 TransportHints() 中将 tcpNoDelay 设置为 true:
node.subscribe(...,ros::TransportHints().tcpNoDelay(true));
请注意,这发生在订阅端。
http://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers#Transport_Hints
相关文章:
- 架构决策:返回std::future还是提供回调
- 正在为Xtensa simcall函数编写回调函数
- 如何在C++中使用非静态成员函数作为回调函数
- FLTK:按下哪个按钮 - 将数字传递给按钮的回调 (lambda)
- 在简单示例中,Python3 + ctypes 回调会导致内存泄漏
- 用于在回调中调用解析器的设计模式
- 如何使用C++对象的成员函数作为 C 样式回调?
- Java从C++回调到C++回调
- 如何将成员函数作为回调参数传递给需要"typedef-ed"自由函数指针的函数?
- 从不同的 cpp 调用回调函数会导致bad_function_call
- pcap_handler回调仅在使用 NPCAP v0.9991 时包含空数据包
- 哪些消息可以传递给低级鼠标挂钩回调函数
- ROS订阅者回调缺少消息
- 在状态栏上显示来自不同线程中回调的消息
- ZeroMQ是否有数据到达时的通知/回调事件/消息
- 事件驱动编程:回调与消息轮询
- WinAPI-带有自己回调的消息循环
- 使用Windows消息循环的回调实现
- 消息传递系统:回调可以是任何东西
- Windows:当我从本机代码回调到托管代码时,将显示“仅消息”窗口