Vision for Robots 2 of n: Colorspace Conversion

If you have not completed step 1 of n, go here first.

Now that we should have ROS setup and working, lets look at our problem. We have a video of two oranges being viewed by a moving camera. We want our robot to be able to interact with these oranges so first we are going to separate these oranges from the background and use circle detection to identify their size and location within the image. The process of separating the oranges from the background will take advantage of the distinct color of the oranges. However since the brightness of the orange is not constant we will first need to convert from blue, green, red (BGR) to HSV and segment the image by hue.

ROS is a robotics framework that provides a way of building software for robots that makes your life easier. It provides two primary means of communications between different programs called nodes; the first method is a client/server interface which is useful for intermittent and asynchronous communications, secondly ROS provides a messaging interface called topics which is ideal for information that is sent from one publisher to many listeners. Using the second method our program will be able to either listen or publish messages to a topic, and in this demo we will be both sending and receiving image messages.

This code is in C++ but there are many other options available for both ROS and OpenCV. This code is somewhat based on this tutorial.

Now, let's get started with the code.

Assuming you completed step 1 of n, you should already have everything downloaded and you can follow along by looking at the source in ihr_demo_hsv.cpp.

cd ~/ros/stacks/iheart-ros-pkg/ihr_demos/ihr_opencv/src


Here the main function handles initializing our objects.

int
main (int argc, char **argv)
{
  // Initialize ROS Node
  ros::init (argc, argv, "ihr_demo1");
  // Start node and create a Node Handle
  ros::NodeHandle nh;
  // Instaniate Demo
  Demo d (nh);
  // Spin ...
  ros::spin ();
  // ... until done
  return 0;
}

All of the code we care about is stored in the Demo object. The protected portion of Demo has all of the variables and objects we will need to process images. CvBridge uses the older IplImage format for connecting ROS to OpenCV. However, the demo will be using the newer cv::Mat format internally to manipulate images. This will make things easier in the future. Looking at the ROS C++ Style Guide will explain why things are named the way they are.

class Demo
{

protected:
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  sensor_msgs::CvBridge bridge_;
  image_transport::Publisher image_pub_;
  cv::Mat img_in_;
  cv::Mat img_hsv_;
  IplImage *cv_input_;
  IplImage cv_output_;

...

};

The public part of the Demo object has the constructor and an image callback that is called each time a new image arrives.

public:

    Demo (ros::NodeHandle & nh):nh_ (nh), it_ (nh_)
  {
    // Advertise image messages to a topic
    image_pub_ = it_.advertise ("/demo/output_image", 1);
    // Listen for image messages on a topic and setup callback
    image_sub_ = it_.subscribe ("/usb_cam/image_raw", 1, &Demo::imageCallback, this);
    // Open HighGUI Window
    cv::namedWindow ("hsv", 1);
  }

  void imageCallback (const sensor_msgs::ImageConstPtr & msg_ptr)
  {
    // Convert ROS Imput Image Message to IplImage
    try
    {
      cv_input_ = bridge_.imgMsgToCv (msg_ptr, "bgr8");
    }
    catch (sensor_msgs::CvBridgeException error)
    {
      ROS_ERROR ("CvBridge Input Error");
    }

    // Convert IplImage to cv::Mat
    img_in_ = cv::Mat (cv_input_).clone ();
    // Convert Input image from BGR to HSV
    cv::cvtColor (img_in_, img_hsv_, CV_BGR2HSV);
    // Display HSV Image in HighGUI window
    cv::imshow ("hsv", img_hsv_);

    // Needed to  keep the HighGUI window open
    cv::waitKey (3);

    // Convert cv::Mat to IplImage
    cv_output_ = img_hsv_;
    // Convert IplImage to ROS Output Image Message and Publish
    try
    {
      image_pub_.publish (bridge_.cvToImgMsg (&cv_output_, "bgr8"));
    }
    catch (sensor_msgs::CvBridgeException error)
    {
      ROS_ERROR ("CvBridge Output error");
    }
  }

Building from source is easy. First install the dependencies.

cd ~/ros/stacks/iheart-ros-pkg/ihr_demos/ihr_opencv
rosdep install ihr_opencv


After all of the dependencies like OpenCV are installed, use rosmake to compile the package.

rosmake ihr_opencv


To launch the HSV conversion demo using prerecorded .bag file

roslaunch ~/ros/stacks/iheart-ros-pkg/ihr_demos/ihr_opencv/launch/demo_hsv.launch


If you camera is working you can try that as an input.

roslaunch ~/ros/stacks/iheart-ros-pkg/ihr_demos/ihr_opencv/launch/live_hsv.launch



Video of Step 2 of n.

Now that the image has been converted the next step will be to segment the image.