ros使用opencv写自己的usb摄像头发布者(publiser)和订阅者(sublisher)

１．首先完成工作空间、编译空间的创立

２．完成功能包的的创建，

catkin_creat_pkg camera_pkg std_msgs roscpp

３．进入camera_pkg/src目录，创建发布的文件并编写代码

touch camera_publisher.cpp
vim camera_publisher.cpp

代码如下：

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

int main(int argc, char** argv)
{
	//ros节点初始化，定义节点名字
	ros::init(argc, argv, "camera_publisher");
	//创建句柄
	ros::NodeHandle n;

	//捕获视频
	const int CAMERA_INDEX = 0;
	cv::VideoCapture capture(CAMERA_INDEX);

	if( not capture.isOpened())
	{
		ROS_ERROR_STREAM("Failed to open camera with index" << CAMERA_INDEX << "!");
		ros::shutdown();
	}
	//创建一个发布者(publisher)发布名为camera的话题(topic),消息队列长度为１
	image_transport::ImageTransport it(n);
	image_transport::Publisher pub_image = it.advertise("camera", 1);
	
	cv_bridge::CvImagePtr frame = boost::make_shared<cv_bridge::CvImage>();
	frame->encoding = sensor_msgs::image_encodings::BGR8;

	while(ros::ok())
	{
		capture >> frame->image;	//获取一帧图像

		if(frame->image.empty())
		{
			ROS_ERROR_STREAM("Failed to capture frame!");
			ros::shutdown();
		}

		frame->header.stamp = ros::Time::now();	//加上时间戳
		pub_image.publish(frame->toImageMsg());	//发布消息
		
		cv::waitKey(3);
		ros::spinOnce();
	}

	capture.release();
	return EXIT_SUCCESS;
}

４.1修改camera_pkg目录下的package.xml，在原有的基础上添加如下代码（即： <!-- 这里是打开package.xml文件手动添加的 -->之后的代码）

  <buildtool_depend>catkin</buildtool_depend>
  <!-- 这里是之前用catkin_creat_pkg中的依赖项产生的　-->
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
　<build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
　<exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <!-- 这里是打开package.xml文件手动添加的　-->
　<build_depend>cv_bridge</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>image_transport</build_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>image_transport</exec_depend>

4.2修改camera_pkg目录下的CMakeLists.txt,

cmake_minimum_required(VERSION 3.0.2)
project(camera_pkg)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
#======================================================================添加如下代码
find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  roscpp
  std_msgs
  image_transport
  sensor_msgs
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
#======================================================================添加如下代码
find_package(OpenCV REQUIRED)


################################################
## Declare ROS messages, services and actions ##
################################################


################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
#======================================================================添加如下代码
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES camera_pkg
  CATKIN_DEPENDS cv_bridge roscpp std_msgs sensor_msgs image_transport
  #DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
#======================================================================添加如下代码
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
#======================================================================添加如下代码
add_executable(${PROJECT_NAME}_node src/camera_publisher.cpp)

## Specify libraries to link a library or executable target against
#======================================================================添加如下代码
target_link_libraries(${PROJECT_NAME}_node
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
)
#############
## Install ##
#############

#############
## Testing ##
#############

５.1进入camera_pkg/src目录，创建订阅者的文件并编写代码

touch camera_subscriber.cpp
vim camera_subscriber.cpp

代码如下

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

//定义回调函数
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
	try
	{
		//显示图像并打印时间戳
		cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
		std::cout << "stamp: " << msg->header.stamp << std::endl;
		cv::waitKey(3);
	}
	catch(cv_bridge::Exception& e)
	{
		ROS_ERROR("Could not convert from '%s' to 'bgr8'.",msg->encoding.c_str());
	}
}

int main(int argc, char** argv)
{
	//初始化节点，定义节点名字
	ros::init(argc, argv, "image_sub_node");
	ros::NodeHandle n;
	//创建窗口用于显示图像
	cv::namedWindow("view");
	cv::startWindowThread();
	//创建一个订阅者，订阅名为camera的话题(topic),　注册回调函数
	image_transport::ImageTransport it(n);
	image_transport::Subscriber sub = it.subscribe("camera", 1, imageCallback);
	//循环等待回调函数
	ros::spin();
	cv::destroyWindow("view");

}

５.2修改camera_pkg目录下的CMakeLists.txt,加入如下代码，由于不需要其他依赖，所以无需在package.xml添加其他依赖了

add_executable(image_sub_node src/camera_subscriber.cpp)
target_link_libraries(image_sub_node
	${catkin_LIBRARIES}
	${OpenCV_LIBS}
)

６.回到最外层目录,(即camera_pkg所在目录的上一层目录），进行编译

catkin_make

7.使用自己写的包。

source devel/setup.bash
rosrun camera_pkg camera_pkg_node

另外开一个终端:
source devel/setup.bash
rosrun camera_pkg image_sub_node