Lecture13. Useful ROS 2 Examples (Sensor Part)

# 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

VIDEO_FILE_ROOT = "/home/kimsooyoung/ros2_ws/rosbag2/img_pub_test.mp4"

# Terminal 1 - pkg build and run
cbp py_cv_tutorial && source install/local_setup.bash
ros2 run py_cv_tutorial img_pub

# Terminal 2 - rqt_image_view
ros2 run rqt_image_view rqt_image_view

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):
        ...
        # Used to convert between ROS and OpenCV images
        self.br = CvBridge()

    def timer_callback(self):
        ...
        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, encoding="bgr8"))

# 예제 실행 준비
colcon build --packages-select py_cv_tutorial
source install/local_setup.bash

# Terminal 1 – 데이터 위치로 이동하여 ros2 bag 실행
cd <데이터를 저장한 위치>
ros2 bag play quadrupped_train.bag_0.db3

# Terminal 2 – image_view 실행
ros2 run rqt_image_view rqt_image_view

# Terminal 1
$ cd <bag 파일 위치>
$ ros2 bag play quadruped_train/quadruped_train.bag_0.db3
[INFO] [1667077909.362223100] [rosbag2_storage]: Opened database 'quadrupped_train/quadrupped_train.bag_0.db3' for READ_ONLY.
...

# Terminal 2
$ ros2 run py_cv_tutorial img_sub
[INFO] [1667077909.613586300] [image_subscriber]: Receiving video frame
[INFO] [1667077910.934147000] [image_subscriber]: Receiving video frame
...

from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library

class ImageSubscriber(Node):

    def __init__(self):

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

		def listener_callback(self, data):
				...
        # Convert ROS Image message to OpenCV image
        current_frame = self.br.imgmsg_to_cv2(data, "bgr8")
        edge_frame = self.hough_transform(current_frame)

$ ros2 run py_cv_tutorial rosbag2_to_timedimg
/home/kimsooyoung/ros2_ws/rosbag2/quadrupped_train/quadrupped_train.bag_0.db3
saved: color_1666796292992515592.png
saved: color_1666796293035428479.png
saved: color_1666796293079139778.png
saved: color_1666796293120768031.png
saved: color_1666796293161406687.png
...

def main(args=None):

    # Change below roots to your ros2 bag locations
    ROOT_DIR = "/home/kimsooyoung/ros2_ws/rosbag2/quadrupped_train"
    FILENAME = "/quadrupped_train.bag_0.db3"
    DESCRIPTION = "color_"

$ pkg-config --modversion opencv4
4.2.0

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/header.hpp"
#include "sensor_msgs/msg/image.hpp"

// cv_bridge converts between ROS 2 image messages and OpenCV image representations.
#include <cv_bridge/cv_bridge.h>
// Using image_transport allows us to publish and subscribe to compressed image streams in ROS2
#include <image_transport/image_transport.hpp>
// We include everything about OpenCV as we don't care much about compilation time at the moment.
#include <opencv2/opencv.hpp>

const std::string VIDEO_FILE_ROOT = "/home/kimsooyoung/ros2_ws/go1_lidar.mp4";

# Terminal 1 - run demo
ros2 run cpp_cv_tutorial img_pub

# Terminal 2 - rqt_image_view
ros2 run rqt_image_view rqt_image_view

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.hpp>
#include <opencv2/opencv.hpp>
...

  void timer_callback() {
    // // Create a new 640x480 image
    // cv::Mat my_image(cv::Size(640, 480), CV_8UC3);

    //--- GRAB AND WRITE LOOP
    cap.read(my_img);
    if (my_img.empty()){
      std::cerr << "ERROR! blank frame grabbed\n";
      rclcpp::shutdown();
    }

    // Write message to be sent. Member function toImageMsg() converts a CvImage
    // into a ROS image message
    img_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", my_img).toImageMsg();

    // Publish the image to the topic defined in the publisher
    image_pub->publish(*img_msg.get());
  }

ros2 launch chess_world chess_world.launch.py

class ImageSubscriber : public rclcpp::Node{
private:
  rclcpp::Subscription<Image>::SharedPtr m_img_sub;
  rclcpp::Publisher<Image>::SharedPtr m_img_pub;
public:
  ImageSubscriber(): Node("image_subscriber"){

    std::string topic_name = "video_frames";

    m_img_sub = this->create_subscription<Image>(
      topic_name, 10, std::bind(&ImageSubscriber::image_cb, this, std::placeholders::_1)
    );

ros2 run cpp_cv_tutorial img_sub

cv::Mat image_processing(const cv::Mat img_in){

  // Create output image
  cv::Mat img_out;
  cv::Mat img_small;
  ...
  return img_out;
}

void image_cb(const Image::SharedPtr msg) const{
  // Convert ROS Image to CV Image
  cv::Mat frame = cv_bridge::toCvCopy(
    msg, sensor_msgs::image_encodings::BGR8
  )->image;

  // Image processing
  cv::Mat proced_img = image_processing(frame);
}

find_package(OpenCV REQUIRED)

add_executable(img_pub src/img_pub.cpp)
ament_target_dependencies(img_pub rclcpp std_msgs sensor_msgs cv_bridge image_transport OpenCV)

add_executable(img_sub src/img_sub.cpp)
ament_target_dependencies(img_sub rclcpp std_msgs sensor_msgs cv_bridge image_transport OpenCV)

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F U
COUNT 1 1 1 1
WIDTH 4421
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 4421
DATA ascii
-0.8739934 -1.342802 0.73930842 1577997
-0.86556709 -1.342802 0.73930842 1577997
-0.85433203 -1.3428019 0.73930848 1577997
-0.84450132 -1.3428019 0.73930854 1577997

std_msgs/Header header
geometry_msgs/Point32[] points
ChannelFloat32[] channels

std_msgs/Header header

uint32 height
uint32 width

PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

$ sudo pip install cython
$ sudo apt-get install pcl-tools
$ sudo apt install python3-pcl

$ sudo add-apt-repository ppa:sweptlaser/python3-pcl
$ sudo apt update
$ sudo apt install python3-pcl

$ sudo apt install python3-pcl

$ python3
Python 3.8.10
[GCC 9.4.0] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> import pcl
>>>

cd py_pcl_tutorial/exercise
python3 example1_load_and_view.py

import pcl
import pcl.pcl_visualization

# Load point cloud file
cloud = pcl.load_XYZRGB('./data/pcl_sub_node.pcd')

# Create pcl built-in viewer
visual = pcl.pcl_visualization.CloudViewing()

##### Visualize APIs #####
# 'ShowColorACloud',
# 'ShowColorCloud',
# 'ShowGrayCloud',
# 'ShowMonochromeCloud',
# 'WasStopped'

filename = 'example_save.pcd'
pcl.save(cloud, filename)

visual.ShowColorCloud(cloud, b'cloud')
v = True
while v:
    v = not(visual.WasStopped())

# Voxel Grid filter
# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()

# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.01

# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)

# Call the filter function to obtain the resultant downsampled point cloud
cloud_voxed = vox.filter()

# Pass Through filter
passthrough = cloud_voxed.make_passthrough_filter()

# Assign axis and range to the passthrough filter object.
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = -1.1
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)

cloud_filtered = passthrough.filter()

# RANSAC plane segmentation
# Create the segmentation object
seg = cloud_filtered.make_segmenter()

# Set the model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)

# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)

# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()

# Extract inliers
extracted_inliers = cloud_filtered.extract(inliers, negative=True)

# Much like the previous filters, we start by creating a filter object:
outlier_filter = cloud_filtered.make_statistical_outlier_filter()

# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(50)

# Set threshold scale factor
x = 1.0

# Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier
outlier_filter.set_std_dev_mul_thresh(x)

# Finally call the filter function for magic
cloud_filtered = outlier_filter.filter()

# create kd tree for the search method of extraction
tree = extracted_inliers.make_kdtree()

# Load point cloud file
cloud = pcl.load_XYZRGB('./data/pcl_sub_node.pcd')

# eliminate color from cloud for clustering
while_cloud = XYZRGB_to_XYZ(cloud)
...

# Create a cluster extraction object
ec = extracted_inliers.make_EuclideanClusterExtraction()

# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.07)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(25000)

# Search the k-d tree for clusters
ec.set_SearchMethod(tree)

# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()

# Create Cluster-Mask Point Cloud to visualize each cluster separately
get_color_list.color_list =[]
cluster_color = get_color_list(len(cluster_indices))

# Assign a color corresponding to each segmented object in scene
color_cluster_point_list = []

for j, indices in enumerate(cluster_indices):
    for i, indice in enumerate(indices):
        color_cluster_point_list.append([extracted_inliers[indice][0],
                                        extracted_inliers[indice][1],
                                        extracted_inliers[indice][2],
                                        rgb_to_float(cluster_color[j])])

#Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)

# Load point cloud file
cloud = pcl.load_XYZRGB('./data/pcl_sub_node.pcd')

# eliminate color from cloud for clustering
while_cloud = XYZRGB_to_XYZ(cloud)

# Create a cluster extraction object
ec = extracted_inliers.make_EuclideanClusterExtraction()

# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.07)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(25000)

ec.set_SearchMethod(tree)

# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()

# Create Cluster-Mask Point Cloud to visualize each cluster separately
get_color_list.color_list =[]
cluster_color = get_color_list(len(cluster_indices))

# Assign a color corresponding to each segmented object in scene
color_cluster_point_list = []

for j, indices in enumerate(cluster_indices):
    for i, indice in enumerate(indices):
        color_cluster_point_list.append([extracted_inliers[indice][0],
                                        extracted_inliers[indice][1],
                                        extracted_inliers[indice][2],
                                        rgb_to_float(cluster_color[j])])

# Terminal 1 - gazebo world launch
ros2 launch lidar_world lidar_world.launch.py

# Terminal 2 - python node run
ros2 run py_pcl_tutorial pcl_cluster_pub
[INFO] [1682602819.908146533] [pcl_cluster_node]: PCL Cluster Node has been started

def ros2_to_pcl(ros_cloud):
    """ Converts a ROS PointCloud2 message to a pcl PointXYZRGB

        Args:
            ros_cloud (PointCloud2): ROS PointCloud2 message

        Returns:
            pcl.PointCloud_PointXYZRGB: PCL XYZRGB point cloud
    """
    ...

def pcl_to_ros2(pcl_array, now=None):
    """ Converts a pcl PointXYZRGB to a ROS PointCloud2 message

        Args:
            pcl_array (PointCloud_PointXYZRGB): A PCL XYZRGB point cloud
            now (Time): A ROS 2 time object like self.get_clock().now()

        Returns:
            PointCloud2: A ROS point cloud
    """

class PCLClusterNode(Node):

    def __init__(self):
        super().__init__('pcl_cluster_node')

        self.pc_subscription = self.create_subscription(
            PointCloud2, 'pointcloud',
            self.pcl_callback, 10
        )

        self.cluster_publisher = self.create_publisher(
            PointCloud2, 'cluster', 10
        )

    def pcl_callback(self, pcl_msg):

        # TODO: Convert ROS msg to PCL data
        pcl_data = ros2_to_pcl(pcl_msg)
        ...

        #Create new cloud containing all clusters, each with unique color
        cluster_cloud = pcl.PointCloud_PointXYZRGB()
        cluster_cloud.from_list(color_cluster_point_list)
        ...

    # Convert PCL data to ROS 2 messages
    cluster_cloud_msg = pcl_to_ros2(cluster_cloud, now=self.get_clock().now())
    self.cluster_publisher.publish(cluster_cloud_msg)