How to include ROS message filter in c++ project

问题

I want to use the ROS message filter code (http://wiki.ros.org/message_filters) in a C++ project I have but I am pretty new to C++ and am confused how exactly to install and include this into my C++ project. Any help would be greatly appreciated.

回答1:

Since you're 'new to C++`, I'll take it from the top, & assume new to cmake as well.

cd ~/path_to/catkin_ws
catkin_create_pkg my_new_pkg roscpp
cd my_new_pkg
sudo apt-get install ros-[version]-message-filters

In package.xml, clean it up and add message_filters (could've done it from catkin_create_pkg, but I assumed you already created it). Your depends should now be (following this)

<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>message_filters</depend><!-- New -->

Edit the CMakeLists.txt, clean up / delete unrelevant sections. I'm assuming you have a node src/filter_node.cpp. While I'm here, I'll point out that the ROS node should really just wrap your real code; all the real math should go in a plain non-ROS cpp file (ex. src/my_math.cpp), and be called by your node. It significantly helps debugging & testing.

set(CPP_FLAGS "-std=c++11 -Wall -Wextra -Werror -Wno-main -O2 ${CMAKE_CPP_FLAGS} ")
set(CPP_DEVEL_FLAGS "-std=c++11 -Wall -Wextra -Wno-main -O0 -g -DDEBUG ${CMAKE_CPP_FLAGS} ")

find_package(catkin REQUIRED COMPONENTS
  roscpp
  message_filters # New
)
catkin_package(
  INCLUDE_DIRS include # To see your new header files
  LIBRARIES my_new_pkg # If you bundle up your math into a package
  CATKIN_DEPENDS
    roscpp
    message_filters # New
#  DEPENDS system_lib # If you need a apt-get installed or system library
)
include_directories(
  include
  ${catkin_INCLUDE_DIRS} # This will include the headers for message_filters
)

# This is optional, but an example
add_library(my_new_pkg src/my_math.cpp)
add_dependencies(my_new_pkg ${catkin_EXPORTED_TARGETS})
target_link_libraries(my_new_pkg m ${catkin_LIBRARIES}) # If you need to link against math.h
set_target_properties(my_new_pkg PROPERTIES COMPILE_FLAGS ${CPP_DEVEL_FLAGS}) # Compile flags

add_executable(filter_node src/filter_node.cpp)
add_dependencies(filter_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(filter_node my_new_pkg ${catkin_LIBRARIES}) # Having roscpp/message_filters in the above two sections means catkin_LIBRARIES expands to those
set_target_properties(filter_node PROPERTIES COMPILE_FLAGS ${CPP_DEVEL_FLAGS}) # Compile flags

Now, the meat of it: here is an example template with message_filters added. I'm going to assume you mean to use the time-synchronizer part of it, because that use case is the most common reason I've used this library. But the example extends easily.

// filter_node.cpp
// Does, say, stereo fusion on time-synced images to make pointcloud

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

// ROS Msgs
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

// Std Libs
#include <string>
#include <vector>

// Custom Lib
#include "my_new_pkg/my_math.h"

// Subscribers (inputs)
//    cam_left_info_sub (sensor_msgs/CameraInfo): "camera_left/camera_info"
//    cam_left_sub (sensor_msgs/Image): "camera_left/image_raw"
//    cam_right_info_sub (sensor_msgs/CameraInfo): "camera_right/camera_info"
//    cam_right_sub (sensor_msgs/Image): "camera_right/image_raw"

// Publishers (outputs)
//    pcl_pub (sensor_msgs/PointCloud2): "camera/pointcloud"

// Parameters (settings)
//    frequency (double): default= 20.0 Hz
//    debug (bool): default= false

// ROS Handles and Publishers
ros::NodeHandle *nh;
ros::NodeHandle *pnh;
ros::Publisher pcl_pub;

// ROS Callbacks
void cam_left_cb(const sensor_msgs::CameraInfo::ConstPtr& pcinfo_msg);
void cam_right_cb(const sensor_msgs::CameraInfo::ConstPtr& pcinfo_msg);
void timer_cb(const ros::TimerEvent&);
void cam_cb(
  const sensor_msgs::Image::ConstPtr& pcleft_msg,
  const sensor_msgs::Image::ConstPtr& pcright_msg
);

// Params
double frequency = 20.0;
bool debug = false;

// Global Variables
sensor_msgs::PointCloud2 pcl_msg;
sensor_msgs::CameraInfo cinfo_left_msg;
sensor_msgs::CameraInfo cinfo_right_msg;
sensor_msgs::Image cleft_msg;
sensor_msgs::Image cright_msg;

int main(int argc, char **argv){
// Init ROS
  ros::init(argc, argv, "filter_node");
  nh = new ros::NodeHandle(); // Public namespace nh
  pnh = new ros::NodeHandle("~"); // Private namespace nh

// Params and Init
  pnh->getParam("frequency", frequency);
  pnh->getParam("debug", debug);

// Subscribers
  // Notice, normal subscribers:
  ros::Subscriber cam_left_info_sub = nh->subscribe("camera_left/camera_info", 1, cam_left_cb);
  ros::Subscriber cam_right_info_sub = nh->subscribe("camera_right/camera_info", 1, cam_right_cb);
  ros::Timer timer = nh->createTimer(ros::Duration(1.0/frequency), timer_cb);
  // message_filters subscribers
  message_filters::Subscriber<sensor_msgs::Image> cam_left_sub(*nh, "camera_left/image_raw", 5);
  message_filters::Subscriber<sensor_msgs::Image> cam_right_sub(*nh, "camera_right/image_raw", 5);
  // This is where the magic happens, then the callback gets registered
  message_filters::TimeSynchronizer <sensor_msgs::Image, sensor_msgs::Image> cam_sync(
    cam_left_sub, cam_right_sub, 1); // 1 here means the most recent sync'd img pair
  cam_sync.registerCallback(boost::bind(&cam_cb, _1, _2)); // Magic: _X up to num subs

// Publishers
  pcl_pub = nh->advertise<sensor_msgs::PointCloud2>("camera/pointcloud", 1);

// Spin & Cleanup
  ros::spin();
  delete nh;
  delete pnh;
  return 0;
}

// This is ineff on big data structures
void cam_left_cb(const sensor_msgs::CameraInfo::ConstPtr& pcinfo_msg){
  cinfo_left_msg = *pcinfo_msg;
}

void cam_right_cb(const sensor_msgs::CameraInfo::ConstPtr& pcinfo_msg){
  cinfo_right_msg = *pcinfo_msg;
}

// This callback will be called whenever they're sync'd up
// You can choose to compute the data right away or store it
// for a more refined computation rate
void cam_cb(
  const sensor_msgs::Image::ConstPtr& pcleft_msg,
  const sensor_msgs::Image::ConstPtr& pcright_msg
){
  cleft_msg = *pcleft_msg;
  cright_msg = *pcright_msg;
}

void timer_cb(const ros::TimerEvent&){
  // call math routine
  // my_math(&cleft_msg.data, &cright_msg.data, &pcl_msg.data);
  // Fill pcl_msg
  pcl_pub.publish(pcl_msg);
}

来源：https://stackoverflow.com/questions/61958092/how-to-include-ros-message-filter-in-c-project