ros之cpp快速搭建

ros之cpp快速搭建

单线程callback模式

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>

ros::Publisher g_cloud_pub;
int g_frame_num = 0;

void callback(const sensor_msgs::PointCloud2ConstPtr input_msg)
{
  std::cout << "enter into callback fuction!" << std::endl;
  pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*input_msg, *input_cloud);

  pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
  std::vector<int> indice;
  pcl::removeNaNFromPointCloud(*input_cloud, *output_cloud, indice);

  sensor_msgs::PointCloud2 output_msg;
  output_msg.header = input_msg->header;
  pcl::toROSMsg(*output_cloud, output_msg);
  g_cloud_pub.publish(output_msg);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "callback");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");
  
  std::string cloud_topic;
  priv_nh.param<std::string>("cloud_topic", cloud_topic, "input_points");
  ros::Subscriber cloud_sub = nh.subscribe(cloud_topic, 10, callback);
  g_cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/output_points", 10);

  ros::spin();
  return 0;
}

多线程callback模式

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CompressedImage.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <iostream>
#include <string>

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr input_msg)
{
  std::cout << "Enter into cloud callback function!" << std::endl;
}

void image0Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image0 callback function!" << std::endl;
}

void image1Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image1 callback function!" << std::endl;
}

void image2Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image2 callback function!" << std::endl;
}

void image3Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image3 callback function!" << std::endl;
}

void image4Callback(const sensor_msgs::CompressedImageConstPtr &input_msg)
{
  std::cout << "Enter into image4 callback function!" << std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "multi callback");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");
  
  std::string cloud_topic;
  priv_nh.param<std::string>("cloud_topic", cloud_topic, " ");

  int camera_num = 0; 
  priv_nh.param<int>("camera_num", camera_num, 1);
  std::vector<std::string> camera_topic_v(camera_num);
  for (size_t i = 0; i < camera_num; ++i)
  {
    std::string camera_topic = "camera"+ std::to_string(i) + "_topic";
    priv_nh.param<std::string>(camera_topic, camera_topic_v[i], "/image_color/compressed");
  }
  
  ros::Subscriber cloud_sub = nh.subscribe(cloud_topic, 10, cloudCallback);
  ros::Subscriber image0_sub = nh.subscribe(camera_topic_v[0], 10,  image0Callback);
  ros::Subscriber image1_sub = nh.subscribe(camera_topic_v[1], 10,  image1Callback);
  ros::Subscriber image2_sub = nh.subscribe(camera_topic_v[2], 10,  image2Callback);
  ros::Subscriber image3_sub = nh.subscribe(camera_topic_v[3], 10,  image3Callback);
  ros::Subscriber image4_sub = nh.subscribe(camera_topic_v[4], 10,  image4Callback);
   
  ros::MultiThreadedSpinner spinner(camera_num + 1);
  spinner.spin();
  return 0;
}

message filter模式

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sensor_msgs/Image.h>

void callback(const sensor_msgs::ImageConstPtr& image1, const sensor_msgs::ImageConstPtr& image2)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");

  message_filters::Subscriber<sensor_msgs::Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<sensor_msgs::Image> image2_sub(nh, "image2", 1);

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}

常用库

# catkin
find_package(catkin REQUIRED COMPONENTS
	roscpp
	image_transport
	cv_bridge
	)
include_directories(${catkin_INCLUDE_DIRS})

# opencv
find_package(OpenCV 3 REQUIRED)
include_directories(${OpenCV_LIBRARIES})

# pcl
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})

# boost
find_package( Boost COMPONENTS system REQUIRED)
include_directories(${Boost_INCLUDE_DIRS})

# executable
add_executable(node src/node.cpp)
target_link_libraries(node 
                      ${catkin_LIBRARIES}
                      ${Boost_LIBRARIES}
                      ${PCL_LIBRARIES}
                      ${OpenCV_LIBRARIES})