# OpenVINO ROS2 Python Demo

## Installing ROS 2 on Ubuntu Linux

https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html

## ROS2 OpenCV Reference

https://automaticaddison.com/getting-started-with-opencv-in-ros-2-foxy-fitzroy-python/

### Build

source /opt/ros/foxy/setup.sh 
source /opt/intel/openvino_2021/bin/setupvars.sh
cd ~/dev_ws/src
ros2 pkg create --build-type ament_python ros2_openvino_py --dependencies rclpy image_transport cv_bridge sensor_msgs std_msgs python3-opencv
cd ~/dev_ws/
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select ros2_openvino_py

### Run Pub

source ~/dev_ws/install/setup.sh
source /opt/intel/openvino_2021/bin/setupvars.sh 
ros2 run cv_basics img_publisher

### Run Sub

source ~/dev_ws/install/setup.sh
source /opt/intel/openvino_2021/bin/setupvars.sh 
ros2 run cv_basics img_subscriber

### Directory
dev_ws/src/ros2_openvino_py/
├── ros2_openvino_py
│   ├── __init__.py
│   ├── webcam_pub.py
│   └── webcam_sub.py
├── package.xml
├── resource
│   └── ros2_openvino_py
├── setup.cfg
├── setup.py
└── test
├── test_copyright.py
├── test_flake8.py
└── test_pep257.py

### src/cv_basics/package.xml

?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ros2_openvino_py</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="[e-mail]">dlcv</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>image_transport</depend>
  <depend>cv_bridge</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>
  <depend>python3-opencv</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

### src/ros2_openvino_py/setup.py

from setuptools import setup

package_name = 'ros2_openvino_py'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='dlcv',
    maintainer_email='[e-mail]',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'img_publisher = cv_basics.webcam_pub:main',
            'img_subscriber = cv_basics.webcam_sub:main',
        ],
    },
)

### src/ros2_openvino_py/ros2_openvino_py/webcam_pub.py

# Basic ROS 2 program to publish real-time streaming 
# video from your built-in webcam
# Author:
# - Addison Sears-Collins
# - https://automaticaddison.com

# Import the necessary libraries
import rclpy # Python Client Library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
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
 
class ImagePublisher(Node):
  """
  Create an ImagePublisher class, which is a subclass of the Node class.
  """
  def __init__(self):
    """
    Class constructor to set up the node
    """
    # Initiate the Node class's constructor and give it a name
    super().__init__('image_publisher')
      
    # Create the publisher. This publisher will publish an Image
    # to the video_frames topic. The queue size is 10 messages.
    self.publisher_ = self.create_publisher(Image, 'video_frames', 10)
      
    # We will publish a message every 0.1 seconds
    timer_period = 0.1  # seconds
      
    # Create the timer
    self.timer = self.create_timer(timer_period, self.timer_callback)
         
    # Create a VideoCapture object
    # The argument '0' gets the default webcam.
    self.cap = cv2.VideoCapture(0)
         
    # Used to convert between ROS and OpenCV images
    self.br = CvBridge()
   
  def timer_callback(self):
    """
    Callback function.
    This function gets called every 0.1 seconds.
    """
    # Capture frame-by-frame
    # This method returns True/False as well
    # as the video frame.
    ret, frame = self.cap.read()
          
    if ret == True:
      # Publish the image.
      # The 'cv2_to_imgmsg' method converts an OpenCV
      # image to a ROS 2 image message
      self.publisher_.publish(self.br.cv2_to_imgmsg(frame))
 
    # Display the message on the console
    self.get_logger().info('Publishing video frame')
  
def main(args=None):
  
  # Initialize the rclpy library
  rclpy.init(args=args)
  
  # Create the node
  image_publisher = ImagePublisher()
  
  # Spin the node so the callback function is called.
  rclpy.spin(image_publisher)
  
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  image_publisher.destroy_node()
  
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
  
if __name__ == '__main__':
  main()

### src/ros2_openvino_py/ros2_openvino_py/webcam_sub.py

# Basic ROS 2 program to publish real-time streaming 
# video from your built-in webcam
# Author:
# - Addison Sears-Collins
# - https://automaticaddison.com

import rclpy # Python library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
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
import numpy as np
from openvino.inference_engine import IECore # OpenVINO 2021.4

class ImageSubscriber(Node):
  """
  Create an ImageSubscriber class, which is a subclass of the Node class.
  """
  def __init__(self):
    """
    Class constructor to set up the node
    """
    # Initiate the Node class's constructor and give it a name
    super().__init__('image_subscriber')
      
    # Create the subscriber. This subscriber will receive an Image
    # from the video_frames topic. The queue size is 10 messages.
    self.subscription = self.create_subscription(
      Image, 
      'video_frames', 
      self.listener_callback, 
      10)
    self.subscription # prevent unused variable warning
      
    # Used to convert between ROS and OpenCV images
    self.br = CvBridge()

    # OpenVINO
    self.threshold = 0.4
    self.ie = IECore()
    # python3 /opt/intel/openvino_2021/deployment_tools/tools/model_downloader/downloader.py --name person-vehicle-bike-detection-2001 -o /home/dlcv/openvino_models
    self.net = self.ie.read_network(model='/home/dlcv/openvino_models/intel/person-vehicle-bike-detection-2001/FP16-INT8/person-vehicle-bike-detection-2001.xml')
    self.input_blob = next(iter(self.net.inputs))
    self.out_blob = next(iter(self.net.outputs))
    batch,channel,height,width  = self.net.inputs[self.input_blob].shape
    self.batch = batch
    self.channel = channel
    self.height = height
    self.width = width
   
  def listener_callback(self, data):
    """
    Callback function.
    """
    # Display the message on the console
    self.get_logger().info('Receiving video frame')

    # Convert ROS Image message to OpenCV image
    frame = self.br.imgmsg_to_cv2(data)

    # OpenVINO    
    image = cv2.resize(frame, (self.width, self.height))
    image = image.transpose((2, 0, 1))  # Change data layout from HWC to CHW
    exec_net = self.ie.load_network(network=self.net, device_name='CPU')
    res = exec_net.infer(inputs={self.input_blob: image})
    size = frame.shape[:2]
    for detection in res['detection_out'][0][0]:
      if detection[2] > self.threshold:
        print("detection[2]:{}".format(detection[2]))  
        xmin = max(int(detection[3]*size[0]), 0)
        ymin = max(int(detection[4]*size[1]), 0)
        xmax = min(int(detection[5]*size[0]), size[0])
        ymax = min(int(detection[6]*size[1]), size[1])
        class_id = int(detection[1])
        cv2.rectangle(frame, (xmin, ymin), (xmax, ymax), (255, 0, 0), 2) # Draw     
 
    # Display image
    cv2.imshow("Object Detection", frame)
    
    cv2.waitKey(1)
  
def main(args=None):
  
  # Initialize the rclpy library
  rclpy.init(args=args)
  
  # Create the node
  image_subscriber = ImageSubscriber()
  
  # Spin the node so the callback function is called.
  rclpy.spin(image_subscriber)
  
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  image_subscriber.destroy_node()
  
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
  
if __name__ == '__main__':
  main()