ros传感器数据及多传感器同步订阅

只是简单的数据订阅功能

订阅图像

CMakeLists.txt

find_package里添加 cv_bridge image_transport

find_package(catkin REQUIRED COMPONENTS
  roscpp cv_bridge image_transport
)

include_directories里面添加 ${OpenCV_INCLUDE_DIRS}

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

在链接库中target_link_libraries添加${OpenCV_LIBRARIES}，例如：

target_link_libraries(camare_topic_name(可执行文件名字)
  ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}
)

程序文件(写在单个cpp中)

头文件

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

订阅图像消息的topic

image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("image_raw", 1, imageCallback);

回调函数，转化成cvMat的格式，之后就可以进行图像处理啦。

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    std::cout<<"time:"<<msg->header<<std::endl;
    cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
    cv::waitKey(30);
}

订阅点云数据

CMakeLists.txt

find_package里添加 pcl_ros

find_package(catkin REQUIRED COMPONENTS
  roscpp pcl_ros
)

程序文件(写在单个cpp中)

头文件

#include <sensor_msgs/PointCloud2.h>

订阅点云消息的topic

ros::Subscriber sub_velo_ = nh.subscribe("/velodyne_points", 1, pointCloudCallback);

回调函数，转化成pcl的格式

void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
    std::cout<<pc_msg->header<<std::endl;
    VPointCloud::Ptr pcl_in_;
    pcl::fromROSMsg(*pc_msg, *pcl_in_)
}

点云ros msg格式转pcl格式

const sensor_msgs::PointCloud2::ConstPtr& points_msg; //ros回调函数里的数据
VPointCloud::Ptr pcl_in_(new VPointCloud); //pcl格式，new VPointCloud必须有，不然程序会报错
pcl::fromROSMsg(*points_msg , *pcl_in_); //转换

多传感器数据同步订阅

按时间戳精确订阅

头文件

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

Subscriber<PointCloud2> cloud_sub_;
Subscriber<Image> image_sub_;
typedef sync_policies::ExactTime<PointCloud2, Image> MySyncPolicy; //用法**importtant**
Synchronizer<MySyncPolicy> sync_;

按时间戳近似同步订阅

实际中，多源传感器很难做到时间戳同步，只能用相近的时间戳来同步
头文件

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h> //多了这两行，不然会报错
#include <message_filters/sync_policies/approximate_time.h>

Subscriber<PointCloud2> cloud_sub_;
Subscriber<Image> image_sub_;
typedef sync_policies::ApproximateTime<PointCloud2 , Image> MySyncPolicy;
Synchronizer<MySyncPolicy> sync_;

cpp里调用：

sync_.registerCallback(boost::bind(&SensorFusion::process, this, _1, _2));

上面两种均省略了命名空间

using namespace sensor_msgs;
using namespace message_filters;