#!/usr/bin/env python3
# Basics ROS program to publish real-time streaming 
# video from your built-in webcam
# Author:
# - Addison Sears-Collins
# - https://automaticaddison.com

# Import the necessary libraries
import rospy # Python library for ROS
from sensor_msgs.msg import Image # Image is the message type
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library
 
def publish_message():

  # Node is publishing to the video_frames topic using 
  # the message type Image
  pub = rospy.Publisher('video_frames', Image, queue_size=10)
	
  # Tells rospy the name of the node.
  # Anonymous = True makes sure the node has a unique name. Random
  # numbers are added to the end of the name.
  rospy.init_node('video_pub_py', anonymous=True)
	
  # Go through the loop 10 times per second
  rate = rospy.Rate(10) # 10hz
	
  # Create a VideoCapture object
  # The argument '0' gets the default webcam.
  cap = cv2.VideoCapture(0)
	
  # Used to convert between ROS and OpenCV images
  br = CvBridge()

  # While ROS is still running.
  while not rospy.is_shutdown():
	
      # Capture frame-by-frame
      # This method returns True/False as well
      # as the video frame.
      ret, frame = cap.read()
		
      if ret == True:
        # Print debugging information to the terminal
        rospy.loginfo('publishing video frame')
			
        # Publish the image.
        # The 'cv2_to_imgmsg' method converts an OpenCV
        # image to a ROS image message
        pub.publish(br.cv2_to_imgmsg(frame))
			
      # Sleep just enough to maintain the desired rate
      rate.sleep()
		
if __name__ == '__main__':
  try:
    publish_message()
  except rospy.ROSInterruptException:
    pass

#!/usr/bin/env python3
# Description:
# - Subscribes to real-time streaming video from your built-in webcam.
#
# Author:
# - Addison Sears-Collins
# - https://automaticaddison.com

# Import the necessary libraries
import rospy # Python library for ROS
from sensor_msgs.msg import Image # Image is the message type
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library

def callback(data):

  # Used to convert between ROS and OpenCV images
  br = CvBridge()

  # Output debugging information to the terminal
  rospy.loginfo("receiving video frame")
  
  # Convert ROS Image message to OpenCV image
  current_frame = br.imgmsg_to_cv2(data)
  
  # Display image
  cv2.imshow("camera", current_frame)
  
  cv2.waitKey(1)
     
def receive_message():

  # Tells rospy the name of the node.
  # Anonymous = True makes sure the node has a unique name. Random
  # numbers are added to the end of the name. 
  rospy.init_node('video_sub_py', anonymous=True)
  
  # Node is subscribing to the video_frames topic
  rospy.Subscriber('video_frames', Image, callback)

  # spin() simply keeps python from exiting until this node is stopped
  rospy.spin()

  # Close down the video stream when done
  cv2.destroyAllWindows()
 
if __name__ == '__main__':
  receive_message()

<launch>
  <node
    pkg="cv_basics"
    type="webcam_pub.py"
    name="webcam_pub" 
    output="screen"
  />
  <node
    pkg="cv_basics"
    type="webcam_sub.py"
    name="webcam_sub"
    output="screen"
  />
</launch>

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

// Author: Addison Sears-Collins
// Website: https://automaticaddison.com
// Description: A basic image publisher for ROS in C++
// Date: June 27, 2020

int main(int argc, char** argv)
{
    ros::init(argc, argv, "video_pub_cpp");
    ros::NodeHandle nh;  // Default handler for nodes in ROS

    // 0 reads from your default camera
    const int CAMERA_INDEX = 0;
    cv::VideoCapture capture(CAMERA_INDEX); 
    if (!capture.isOpened()) {
      ROS_ERROR_STREAM("Failed to open camera with index " << CAMERA_INDEX << "!");
      ros::shutdown();
    }
    
    // Image_transport is responsible for publishing and subscribing to Images
    image_transport::ImageTransport it(nh);
    
    // Publish to the /camera topic
    image_transport::Publisher pub_frame = it.advertise("camera", 1);
    
    cv::Mat frame;//Mat is the image class defined in OpenCV
    sensor_msgs::ImagePtr msg;

    ros::Rate loop_rate(10);

    while (nh.ok()) {

      // Load image
      capture >> frame; 
   
      // Check if grabbed frame has content
      if (frame.empty()) {
        ROS_ERROR_STREAM("Failed to capture image!");
        ros::shutdown();
      }

      // Convert image from cv::Mat (OpenCV) type to sensor_msgs/Image (ROS) type and publish
      msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
      pub_frame.publish(msg);
      /*
      Cv_bridge can selectively convert color and depth information. In order to use the specified
      feature encoding, there is a centralized coding form:
        Mono8: CV_8UC1, grayscale image
        Mono16: CV_16UC1, 16-bit grayscale image
        Bgr8: CV_8UC3 with color information and the order of colors is BGR order
        Rgb8: CV_8UC3 with color information and the order of colors is RGB order
        Bgra8: CV_8UC4, BGR color image with alpha channel
        Rgba8: CV_8UC4, CV, RGB color image with alpha channel
      */
      //cv::imshow("camera", image);
      cv::waitKey(1); // Display image for 1 millisecond

      ros::spinOnce();
      loop_rate.sleep();
    }  

    // Shutdown the camera
    capture.release();
}

find_package(OpenCV)

include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(webcam_pub_cpp src/webcam_pub_cpp.cpp)
target_link_libraries(webcam_pub_cpp ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

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

// Author: Addison Sears-Collins
// Website: https://automaticaddison.com
// Description: A basic image subscriber for ROS in C++
// Date: June 27, 2020

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{

  // Pointer used for the conversion from a ROS message to 
  // an OpenCV-compatible image
  cv_bridge::CvImagePtr cv_ptr;
  
  try
  { 
  
    // Convert the ROS message  
    cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
    
    // Store the values of the OpenCV-compatible image
    // into the current_frame variable
    cv::Mat current_frame = cv_ptr->image;
    
    // Display the current frame
    cv::imshow("view", current_frame); 
    
    // Display frame for 30 milliseconds
    cv::waitKey(30);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  // The name of the node
  ros::init(argc, argv, "frame_listener");
  
  // Default handler for nodes in ROS
  ros::NodeHandle nh;
  
  // Used to publish and subscribe to images
  image_transport::ImageTransport it(nh);
  
  // Subscribe to the /camera topic
  image_transport::Subscriber sub = it.subscribe("camera", 1, imageCallback);
  
  // Make sure we keep reading new video frames by calling the imageCallback function
  ros::spin();
  
  // Close down OpenCV
  cv::destroyWindow("view");
}

add_executable(webcam_sub_cpp src/webcam_sub_cpp.cpp)
target_link_libraries(webcam_sub_cpp ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

<launch>
  <node
    pkg="cv_basics"
    type="webcam_pub_cpp"
    name="webcam_pub_cpp" 
    output="screen"
  />
  <node
    pkg="cv_basics"
    type="webcam_sub_cpp"
    name="webcam_sub_cpp"
    output="screen"
  />
</launch>