ros2 message filter
-
系统UBUNTU22.04,ROS2 HUMBLE
各位大佬,我使用Message filter想做时间软同步,代码如下:#include <memory> #include <stdio.h> #include <stdlib.h> #include <string> #include <cstring> #include "rclcpp/rclcpp.hpp" #include <message_filters/subscriber.h> #include <message_filters/time_synchronizer.h> #include <message_filters/synchronizer.h> #include <message_filters/sync_policies/approximate_time.h> #include "dvl_msgs/msg/odom.hpp" #include "ahrs_msgs/msg/fn70_auxa.hpp" #include "plc_msgs/msg/sensor.hpp" using namespace message_filters; using std::placeholders::_1; using std::placeholders::_2; using std::placeholders::_3; class AUVSENSORS_MESSAGES : public rclcpp::Node { public: AUVSENSORS_MESSAGES() : Node("auvsensors_message") { odom_sub_.subscribe(this,"/dvl/odom"); imu_sub_.subscribe(this,"/ahrs/raw"); depth_sub_.subscribe(this,"/plc"); sync_ = std::make_shared<message_filters::TimeSynchronizer<dvl_msgs::msg::Odom, ahrs_msgs::msg::Fn70Auxa, plc_msgs::msg::Sensor>>(odom_sub_, imu_sub_, depth_sub_,10); sync_->registerCallback(std::bind(&AUVSENSORS_MESSAGES::disparityCb, this, _1, _2, _3)); } private: void disparityCb(const dvl_msgs::msg::Odom::ConstSharedPtr& dvl_msg, const ahrs_msgs::msg::Fn70Auxa::ConstSharedPtr& imu_msg,const plc_msgs::msg::Sensor::ConstSharedPtr& ph_msg) { const char *t_1 = std::to_string(dvl_msg->linear.x).c_str(); const char *t_2 = std::to_string(dvl_msg->linear.x).c_str(); const char *t_3 = std::to_string(dvl_msg->linear.x).c_str(); const char *t_4 = std::to_string(imu_msg->altitude).c_str(); const char *t_5 = std::to_string(ph_msg->depth).c_str(); RCLCPP_INFO(this->get_logger(), "消息:%s,%s,%s,%s,%s", t_1,t_2,t_3,t_4,t_5); } message_filters::Subscriber<dvl_msgs::msg::Odom> odom_sub_ ; message_filters::Subscriber<ahrs_msgs::msg::Fn70Auxa> imu_sub_; message_filters::Subscriber<plc_msgs::msg::Sensor> depth_sub_; std::shared_ptr<message_filters::TimeSynchronizer<dvl_msgs::msg::Odom, ahrs_msgs::msg::Fn70Auxa, plc_msgs::msg::Sensor>> sync_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<AUVSENSORS_MESSAGES>()); rclcpp::shutdown(); return 0; }
我想输出几个实例数据,因此我运行记录数据的bag包,其能够正常play,
os2 bag play rosbag2_2023_12_08-15_36_13 [INFO] [1702044100.446161521] [rosbag2_storage]: Opened database 'rosbag2_2023_12_08-15_36_13/rosbag2_2023_12_08-15_36_13_0.db3' for READ_ONLY. [INFO] [1702044100.446240068] [rosbag2_player]: Set rate to 1 [INFO] [1702044100.456354394] [rosbag2_player]: Adding keyboard callbacks. [INFO] [1702044100.456406646] [rosbag2_player]: Press SPACE for Pause/Resume [INFO] [1702044100.456417656] [rosbag2_player]: Press CURSOR_RIGHT for Play Next Message [INFO] [1702044100.456426625] [rosbag2_player]: Press CURSOR_UP for Increase Rate 10% [INFO] [1702044100.456434797] [rosbag2_player]: Press CURSOR_DOWN for Decrease Rate 10% [INFO] [1702044100.456868918] [rosbag2_storage]: Opened database 'rosbag2_2023_12_08-15_36_13/rosbag2_2023_12_08-15_36_13_0.db3' for READ_ONLY.
但是我订阅其话题并实现时间同步输出实例参数的终端报以下错误,这是怎么回事,怎样解决呢?
terminate called after throwing an instance of 'std::runtime_error' what(): can't compare times with different time sources [ros2run]: Aborted