如何在c++项目中包含ROS消息过滤器

Question

我想在我已有的C++项目中使用ROS消息过滤器代码(http://wiki.ros.org/message_filters)，但我对C++还很陌生，不知道如何安装它并将其包含到我的C++项目中。任何帮助都将不胜感激。

Answer 1

由于您是C++`的新手，我将从头开始介绍，并假定cmake也是新的。

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

在package.xml中，清理它并添加message_filters (也可以在catkin_create_pkg中完成，但我假设您已经创建了它)。您的依赖项现在应该是(如下所示)

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

编辑CMakeLists.txt，清理/删除不相关的部分。我假设你有一个节点src/filter_node.cpp。在这里，我将指出ROS节点实际上应该包装您的实际代码；所有真正的数学运算都应该放在一个普通的非ROS cpp文件中(例如，src/my_math.cpp)，并由您的节点调用。它对调试和测试有很大的帮助。

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

现在，重点是:这是一个添加了message_filters的示例模板。我假设您打算使用它的时间同步器部分，因为这个用例是我使用这个库的最常见的原因。但是这个例子很容易扩展。

// 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);
}