ROSの画像転送を軽くするための実験 Part2

# 0. はじめに

前回、OpenCVの圧縮処理でデータ転送量を軽量化する実験をしました。

https://qiita.com/wakaba130/items/740379521cf3c16e36fb

今回、やっと`image_transport`の使い方がわかったので、試して行きたいと思います。
実験で使用した画像や実験環境は、Part1で使用したものと同じです。


# 1. 結論

image_transportに関しても、内部処理でJPEG圧縮やPNG圧縮をかけているだけだったので、Part1の結果とあまり変わりませんでした。RosTopicの`/image_converter/compressed`が圧縮画像です。rqtの機能でTopicの中を確認すると、formatの欄でJPEG圧縮がかかっていることがわかります。
`/image_converter`のTopicは、無圧縮の画像になります。

![Screenshot from 2023-05-07 07-33-47_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/104675/8c8937d0-02af-da4e-21df-ab88b4b9f9a7.png)

## 圧縮なし

Part1と同じく、約90~92MB/sとなっています。

![Screenshot from 2023-05-07 03-28-54.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/104675/765bc603-70d7-e5d4-a7f4-7769d9b5fc77.png)

## image_transport(JPEG圧縮)あり

約19MB/sとなり、Part1で試したJPEG圧縮とほぼ変わりない結果となりました。
JPEG圧縮なので、転送先で画像が若干劣化します。

![Screenshot from 2023-05-07 18-42-08.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/104675/6102cb6e-4bf2-6dcd-0758-6b9aa9142674.png)

また、Pythonが対応してないという情報がありましたが、最終的にはPythonでの実行方法も見つけました
（がんばってC++で書いて確認したのに）。

http://wiki.ros.org/ja/rospy_tutorials/Tutorials/WritingImagePublisherSubscriber

----

# 2. 使い方

ここからは、Publisher側とSubscriber側での`image_transport`の使い方について記載します。
メモ書き程度なので、Part1で説明している環境構築やDockerの実行環境などは割愛しています。

## 2.1. Publisher側

Publisher側のノードを作成するために、パッケージを作成します。

```bash
$ cd /work/catkin_ws
$ source /opt/ros/noetic/setup.bash
$ source devel/setup.bash
$ cd src
$ catkin_create_pkg imgpub_test roscpp std_msgs sensor_msgs cv_bridge image_transport compressed_image_transport
$ cd imgpub_test/src
$ touch imgpub_test_node.cpp
```

上記のコマンドを打つと、下記のようなディレクトリ構造になると思います。

```
.
├── build
├── devel
└── src
    ├── CMakeLists.txt
    └── imgpub_test
        ├── CMakeLists.txt
        ├── include
        │   └── imgpub_test
        ├── package.xml
        └── src
            └── imgpub_test_node.cpp
```

`imgpub_test_node.cpp`の中を記載します。

```cpp:imgpub_test_node.cpp
#include <iostream>
#include <chrono>
#include "ros/ros.h"
#include "image_transport/image_transport.h"
#include <cv_bridge/cv_bridge.h>
#include "sensor_msgs/image_encodings.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

class ImagePublisher
{
    ros::NodeHandle nh;
    ros::Publisher image_pub;
    
public:
    ImagePublisher()
    {
        image_pub = nh.advertise<sensor_msgs::Image>("img_color", 1);
    }

    ~ImagePublisher(){}

    void publish_img()
    {
        cv::Mat img = cv::imread("/data/DSC_0040.JPG");
        cv_bridge::CvImage cv_img;
        try{
            cv_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img);
        }
        catch (cv_bridge::Exception& e){
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }
        image_pub.publish(cv_img.toImageMsg());
    }

};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "img_pub");
    ImagePublisher img_pub;
    while(ros::ok())
    {
        auto start = std::chrono::system_clock::now();
        img_pub.publish_img();
        auto end = std::chrono::system_clock::now();
        auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
        std::cout << msec << " milli sec \n";
    }
    return 0;
}
```

`imgpub_test/CMakeLists.txt`の中身を編集します。

```diff_c_make:imgpub_test/CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(imgpub_test)

## 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
+  compressed_image_transport
+  cv_bridge
+  image_transport
+  roscpp
+  sensor_msgs
+  std_msgs
+ )


## System dependencies are found with CMake's conventions
+ find_package(Boost REQUIRED)
+ find_package(OpenCV REQUIRED)

## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

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

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   sensor_msgs#   std_msgs
# )

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

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## 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 imgpub_test
#  CATKIN_DEPENDS compressed_image_transport cv_bridge image_transport roscpp sensor_msgs std_msgs
#  DEPENDS system_lib
)

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

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/imgpub_test.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## 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/imgpub_test_node.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
+ target_link_libraries(${PROJECT_NAME}_node
+    ${catkin_LIBRARIES}
+ )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

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

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_imgpub_test.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

```

ワークスペースの上位まで戻って、makeします。

```bash
$ cd /work/catkin_ws
$ catkin_make
####
#### Running command: "make cmake_check_build_system" in "/work/catkin_ws/build"
####
####
#### Running command: "make -j20 -l20" in "/work/catkin_ws/build"
####
[100%] Built target imgpub_test_node
```

rosrunで実行すると、画像がPublishされます。

```bash
$ byobu
$ source /opt/ros/noetic/setup.bash
$ source devel/setup.bash
$ export ROS_IP=192.168.10.107
$ roscore
# F2キーで新しいターミナルを開く
$ source /opt/ros/noetic/setup.bash
$ source devel/setup.bash
$ export ROS_IP=192.168.10.107
$ rosrun imgpub_test imgpub_test_node
```

## 2.1. Subscriber側

Subscriber側も同様に、パッケージを作成して、コードを記載して、makeします。

```bash
$ cd /work/catkin_ws
$ source /opt/ros/noetic/setup.bash
$ source devel/setup.bash
$ cd src
$ catkin_create_pkg imgsub_test roscpp std_msgs sensor_msgs cv_bridge image_transport compressed_image_transport
$ cd imgsub_test/src
$ touch imgsub_test_node.cpp
```

```cpp:imgsub_test_node.cpp
#include <iostream>
#include <chrono>
#include "ros/ros.h"
#include "image_transport/image_transport.h"
#include <cv_bridge/cv_bridge.h>
#include "sensor_msgs/image_encodings.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

void imageCallback(const sensor_msgs::CompressedImageConstPtr& msg)
{
    try
    {
        cv::Mat image = cv::imdecode(cv::Mat(msg->data), 1);//convert compressed image data to cv::Mat
        cv::Mat src_img = cv::imread("/data/DSC_0040.JPG");

        cv::Mat imgf1, imgf2;
        image.convertTo(imgf1, CV_32F);
        src_img.convertTo(imgf2, CV_32F);
        auto val = cv::sum(cv::abs(imgf1 - imgf2));

        std::cout << "sum = " << val << std::endl;

        cv::imshow("view", image);
        cv::waitKey(10);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert to image!");
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "image_listener");
    ros::NodeHandle nh;
    cv::namedWindow("view");
    cv::startWindowThread();
    ros::Subscriber sub = nh.subscribe("/img_converter/compressed", 1, imageCallback);
    ros::spin();
    cv::destroyWindow("view");
}
```

`CMakeLists.txt`の編集はPublisher側と同じなので割愛

```bash
$ cd /work/catkin_ws
$ catkin_make
$ source /opt/ros/noetic/setup.bash
$ source ./devel/setup.bash
$ export ROS_MASTER_URI=http://192.168.10.107:11311 # Publisher側のIP
$ export ROS_IP=192.168.10.103 # Subscriber側のIP
$ rosrun imgsub_test imgsub_test_node
```


# 参考

## compressedimage関係

https://wiki.ros.org/compressed_image_transport

http://library.isr.ist.utl.pt/docs/roswiki/image_transport.html

https://answers.ros.org/question/230476/how-to-subscribe-to-sensor_msgscompressedimage-without-the-raw-image/

https://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages

## Qiita記事

https://qiita.com/srs/items/dc72dc81c1ae46856ac5

https://qiita.com/srs/items/de7bf71b96dc26b1f53b

https://qiita.com/Yuya-Shimizu/items/10804c9ac44a3568d116

## その他

https://rb-station.com/blogs/article/rospy-image-opencv

http://vivi.dyndns.org/tech/cpp/timeMeasurement.html

https://answers.ros.org/question/9765/how-to-convert-cvmat-to-sensor_msgsimageptr/

https://demura.net/education/lecture/12480.html

https://sy-base.com/myrobotics/ros/ros_cvbridge/