Writing Your Own FV Plugin (ROS)

Abstract 概要

In this tutorial, we make a simple plugin of FingerVision video processing as a ROS node.

Outline †

We implement a simple FingerVision plugin. It detects red-colored pixels in the input image, computes the ratio of the area, and publishes the ratio as a ROS topic.

What we will create is something like this:

Step-by-Step Tutorial †

Creating a ROS Package †

Create a ROS package. We use the rosbuild framework (not Catkin).

We assume the package name is fv_ros_example. There is an example implementation tutorial/fv_ros_example, but we recommend to implement it by yourself.

fv_ros_example/CMakeLists.txt:

Add the library search:

# ++++OPENCV-2.x++++
find_package(OpenCV 2 REQUIRED)
message("OpenCV_INCLUDE_DIRS: ${OpenCV_INCLUDE_DIRS}")
message("OpenCV_LIBRARIES: ${OpenCV_LIBRARIES}")
# ----OPENCV----

find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

Add the executable:

rosbuild_add_executable(color_detect_node
      src/color_detect_node.cpp
      )
target_link_libraries(color_detect_node
      ay_vision
      ${OpenCV_LIBRARIES}
      )

Note that color_detect_node can be any name.

fv_ros_example/Makefile:

include $(shell rospack find mk)/cmake.mk

fv_ros_example/manifest.xml:

<package>
  <description brief="fv_ros_example">

     fv_ros_example

  </description>
  <author>Akihiko Y</author>
  <license>Described in LICENSE</license>
  <review status="unreviewed" notes=""/>
  <url>http://akihikoy.net/p/fv</url>
  <depend package="fingervision"/>
  <depend package="roscpp"/>
  <depend package="std_msgs"/>
  <depend package="std_srvs"/>

</package>

The important element is the dependency on the fingervision package.

Template †

A good starting point to write a code of the node is using a template.

In the tutorial/fv_ros_example/src directory, there is a template tutorial/fv_ros_example/src/template_std.cpp.

//-------------------------------------------------------------------------------------------
/*! \file    template_std.cpp
    \brief   Template of a FingerVision video processing ROS node.
    \author  Akihiko Yamaguchi, info@akihikoy.net
    \version 0.1
    \date    Aug.20, 2018
*/
//-------------------------------------------------------------------------------------------
#include "ay_vision/vision_util.h"
#include "ay_cpp/sys_util.h"
//-------------------------------------------------------------------------------------------
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ros/ros.h>
//-------------------------------------------------------------------------------------------
using namespace std;
using namespace trick;
//-------------------------------------------------------------------------------------------

cv::Mat Capture(cv::VideoCapture &cap, TCameraInfo &info, TCameraRectifier *pcam_rectifier=NULL)
{
  cv::Mat frame;
  while(!cap.read(frame))
  {
    if(CapWaitReopen(info,cap)) continue;
    else  return cv::Mat();
  }
  if(info.CapWidth!=info.Width || info.CapHeight!=info.Height)
    cv::resize(frame,frame,cv::Size(info.Width,info.Height));
  if(info.HFlip)  cv::flip(frame, frame, /*horizontal*/1);
  Rotate90N(frame,frame,info.NRotate90);
  if(info.Rectification && pcam_rectifier)
    pcam_rectifier->Rectify(frame, /*border=*/cv::Scalar(0,0,0));
  return frame;
}
//-------------------------------------------------------------------------------------------

int main(int argc, char**argv)
{
  ros::init(argc, argv, "template_std");
  ros::NodeHandle node("~");
  std::string cam_config("config/fv_1.yaml");
  std::string vout_base("/tmp/vout-");

  node.param("cam_config",cam_config,cam_config);
  node.param("vout_base",vout_base,vout_base);
  std::cerr<<"cam_config: "<<cam_config<<std::endl;

  std::vector<TCameraInfo> cam_info;
  if(FileExists(cam_config))
    ReadFromYAML(cam_info, cam_config);
  else
  {
    std::cerr<<"Cannot open cam_config: "<<cam_config<<std::endl;
    return -1;
  }

  cv::VideoCapture cap;
  if(!CapOpen(cam_info[0], cap))  return -1;

  // Creating an image rectification object.
  TCameraRectifier cam_rectifier;
  if(cam_info[0].Rectification)
  {
    cv::Size size_in(cam_info[0].Width,cam_info[0].Height), size_out(cam_info[0].Width,cam_info[0].Height);
    cam_rectifier.Setup(cam_info[0].K, cam_info[0].D, cam_info[0].R, size_in, cam_info[0].Alpha, size_out);
  }

  // Video recording tool.
  TEasyVideoOut vout;
  vout.SetfilePrefix("/tmp/cam");

  // [[[Setup publisher, service server, etc.
  // pub= ...
  // Done.]]]

  bool running(true);
  cv::Mat frame;
  for(int f(0);ros::ok();++f)
  {
    if(running)
    {
      frame= Capture(cap, cam_info[0], &cam_rectifier);

      // [[[Process the image:
      // processing frame...
      // Done]]]

      // [[[Publish the result:
      // pub.publish(...);
      // Done]]]

      // Video recording code (necessary even when video is not recorded):
      vout.Step(frame);
      vout.VizRec(frame);  // Draw a recording mark when video is recorded.
      // Displaying image:
      cv::imshow("camera", frame);

    }  // running
    else
    {
      usleep(200*1000);
    }

    // Keyboard event handling:
    char c(cv::waitKey(10));
    if(c=='\x1b'||c=='q') break;
    else if(char(c)=='W')  vout.Switch();  // Start/stop video recording.
    else if(char(c)==' ')
    {
      running=!running;
      std::cerr<<(running?"Resume":"Pause (Hit space/R-click to resume)")<<std::endl;
    }

    ros::spinOnce();
  }

  return 0;
}
//-------------------------------------------------------------------------------------------

There are commented-out sections:

// [[[Setup publisher, service server, etc.
// pub= ...
// Done.]]]

frame= Capture(cap, cam_info[0], &cam_rectifier);

// [[[Process the image:
// processing frame...
// Done]]]

// [[[Publish the result:
// pub.publish(...);
// Done]]]

Implementing them is our task.

For understanding the other part, reading Video Capture and Rectification would be helpful.

Color Detection Function †

Let's define a function to detect red colored pixels.

double DetectColor(cv::Mat &frame)
{
  cv::Mat frame_hsv, mask1, mask2,mask, img_disp;
  cv::cvtColor(frame, frame_hsv, cv::COLOR_BGR2HSV);
  cv::inRange(frame_hsv, cv::Scalar(0, 70, 50), cv::Scalar(20, 255, 255), mask1);
  cv::inRange(frame_hsv, cv::Scalar(160, 70, 50), cv::Scalar(180, 255, 255), mask2);
  mask= mask1 | mask2;
  img_disp= 0.3*frame;
  frame.copyTo(img_disp, mask);
  frame= img_disp;
  return double(cv::countNonZero(mask))/(mask.cols*mask.rows);
}

In this code, an image (frame) is converted to HSV color space, and red-colored pixels are detected as mask. The function returns the ratio of mask over entire area of the image. In the function, the input frame is replaced by an image for display (img_disp) where the detected mask is highlighted.

Implement the Node with the Function †

We create a publisher of std_msgs/Float64. The above function is applied to the image obtained in each iteration, and the returned value is published.

We need to include the header for std_msgs/Float64:

#include <std_msgs/Float64.h>

Let's change the node name:

ros::init(argc, argv, "color_detect_node");

The commented-out sections are implemented like:

// [[[Setup publisher, service server, etc.
ros::Publisher pub_ratio= node.advertise<std_msgs::Float64>("color_ratio", 1);
// Done.]]]

frame= Capture(cap, cam_info[0], &cam_rectifier);

// [[[Process the image:
double ratio= DetectColor(frame);
// Done]]]

// [[[Publish the result:
std_msgs::Float64 msg;
msg.data= ratio;
pub_ratio.publish(msg);
// Done]]]

Build †

Just use rosmake to build the package.

 $ rosmake fv_ros_example

Config Files †

We create a config directory in the new ROS package directory for storing config files of camera devices (or video streams).

In this tutorial, we just make a symbolic link to fingervision/config directory:

 $ ln -s ../../fingervision/config

Execution †

Run roscore on a terminal:

 $ roscore

Then run:

 $ rosrun fv_ros_example color_detect_node _cam_config:=CONFIG_FILE

where CONFIG_FILE is a configuration file in YAML (e.g. config/fv_1.yaml). It should include camera configuration (CameraInfo).

The program has a keyboard interface:

 Press 'q' or Esc: Exit the program.
 Press 'W' (shift+'w'): Start/stop video recording.
 Press ' ': Pause/resume the processing.

Execution Examples †

Using a FingerVision connected to video-1 (/dev/video1) with image rectification:

 $ rosrun fv_ros_example color_detect_node _cam_config:=config/fv_1.yaml

Let's use Dummy FingerVision Data. On another terminal, run:

 $ cd fingervision/tools/
 $ ./stream_file1.sh 

Then run the node with the different configuration:

 $ rosrun fv_ros_example color_detect_node _cam_config:=config/fv_3_l.yaml

You will see some red things are detected and highlighted, such as a flower and a red pen.

It looks like:

In order to see the topics, use the rostopic tool to echo:

 $ rostopic echo /color_detect_node/color_ratio
 data: 0.000455729166667
 ---
 data: 0.000455729166667
 ---
 ...
 ---
 data: 0.212317708333
 ---
 data: 0.214283854167

You will see the value increases when a red thing is in the scene.