Skip to content

Example : Using image and Controlling ROBOTIS OP2 LED

Jump to bottom Edit New page
ROBOTIS-Kayman edited this page Dec 1, 2015 · 26 revisions

1. Process image and control OP LED

This tutorial describes how to interface ROS and OpenCV by converting ROS images into OpenCV images and how to control OP LED. Included is a sample node that can be used as a template for your own node.

This package subscribes ROS image(sensor_msgs/Image) and calculates the RGB average value of center of image. And It controls the LED of ROBOTIS OP to same color. (It works with example_read_write of robotis_example package and robotis_controller package)

###1.1 Installation Get source code

$ cd %YOUR_CATKIN_WORKSPACE_HOME%/src

$ git clone https://github.com/ROBOTIS-GIT/ROBOTIS-OP.git

Build package

$ cd %YOUR_CATKIN_WORKSPACE_HOME%/

$ catkin_make

###1.2 Running the package

$ roslaunch color_recognition color_recognition_op.launch

##2. Nodes ###2.1 color_recognition_node 2.1.1 Subscribed Topics

  • image_raw (sensor_msgs/Image) : The image topic. Should be remapped to the name of the real image topic.

2.1.2 Published Topics

2.1.3 Parameters

  • None

##3. Code ###3.1 color_recognition.h

//std
#include <string>

//ros dependencies
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/ColorRGBA.h>
#include <sensor_msgs/image_encodings.h>

const unsigned int IMG_MONO = 0;
const unsigned int IMG_RGB8 = 1;

class ColorRecogNode
{
protected:

    //ros node handle
    ros::NodeHandle nh;

    //image publisher/subscriber
    image_transport::ImageTransport it;
    image_transport::Publisher imagePub;
    image_transport::Subscriber imageSubs;
    cv_bridge::CvImagePtr cvImgPtr;

    // led color publisher
    std_msgs::ColorRGBA colorMsg;
    ros::Publisher colorPub;

    //flag indicating a new image has been received
    bool newImageFlag;

    //img encoding id
    unsigned int imgEncoding;
    int recog_size;
    int image_height, image_width;
    int start_height, start_width;

protected:
    //callbacks to image subscription
    void imageCallback(const sensor_msgs::Image::ConstPtr &msg);

public:
    //constructor
    ColorRecogNode();

    //destructor
    ~ColorRecogNode();

    //checks if a new image has been received
    bool newImage();

    //execute circle detection with the cureent image
    void process();

    //publish the output image (input image + marked circles)
    void publishImage();

    //publish the circle set data
    void publishColor();
};

Let's break down the above header file.

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

Using image_transport for publishing and subscribing to images in ROS allows you to subscribe to compressed image streams. Remember to include image_transport in your package.xml.

Includes the header for CvBridge as well as some useful constants and functions related to image encodings. Remember to include cv_bridge in your package.xml.

//image publisher/subscriber
image_transport::ImageTransport it;
image_transport::Publisher imagePub;
image_transport::Subscriber imageSubs;
cv_bridge::CvImagePtr cvImgPtr;

// led color publisher
std_msgs::ColorRGBA colorMsg;
ros::Publisher colorPub;

Subscribe to an image topic "in" and advertise an image topic "out" using image_transport. And advertise a color topic to control OP led (colorPub).

###3.2 color_recognition_main.cpp

#include "color_recognition/color_recognition.h"
//node main
int main(int argc, char **argv)
{
      //init ros
      ros::init(argc, argv, "color_recognition_node");

      //create ros wrapper object
      ColorRecogNode color_node;

      //set node loop rate
      ros::Rate loop_rate(30);

      ROS_INFO("Start color recongnition .....");

      //node loop
      while ( ros::ok() )
      {
            //if new image , do things
            if ( color_node.newImage() )
            {
                  color_node.process();
                  color_node.publishImage();
                  color_node.publishColor();
            }

            //execute pending callbacks
            ros::spinOnce();

            //relax to fit output rate
            loop_rate.sleep();
      }

      //exit program
      return 0;
}

Let's break down the above code.

//set node loop rate
ros::Rate loop_rate(30);

while ( ros::ok() )
{
    …
    //relax to fit output rate
    loop_rate.sleep();
}

A ros::Rate object allows you to specify a frequency that you would like to loop at(in the above code, a loop rate is 30hz). It will keep track of how long it has been since the last call to Rate::sleep(), and sleep for the correct amount of time.

###3.3 color_recognition.cpp

#include "color_recognition/color_recognition.h"

ColorRecogNode::ColorRecogNode() : nh(ros::this_node::getName()) , it(this->nh)
{
    //sets publishers
    imagePub = it.advertise("/image_out", 100);
    colorPub = nh.advertise<std_msgs::ColorRGBA>("/head_led_color", 100);      // for head led
    // colorPub = nh.advertise<std_msgs::ColorRGBA>("eye_led_color", 100);     // for eye leds

    //sets subscribers
    imageSubs = it.subscribe("/image_raw", 1, &ColorRecogNode::imageCallback, this);

    //initializes newImageFlag
    newImageFlag = false;
}
ColorRecogNode::~ColorRecogNode()
{
}
bool ColorRecogNode::newImage()
{
    if ( newImageFlag )
    {
        newImageFlag = false;
        return true;
    }
    else
    {
        return false;
    }
}
void ColorRecogNode::process()
{
    if(this->imgEncoding != IMG_RGB8) return;

    // get average of rgb value
    if ( cvImgPtr != NULL )
    {
        int _sum_r = 0, _sum_g = 0, _sum_b = 0;
        for(int _row = start_height; _row < start_height + recog_size; _row++)
        {
            for(int _col = start_width; _col < (start_width + recog_size); _col ++)
            {
                // split rgb channel
                cv::Vec3b _bgr_color = cvImgPtr->image.at<cv::Vec3b>(_row, _col);
                _sum_r += (int) _bgr_color.val[0];
                _sum_g += (int) _bgr_color.val[1];
                _sum_b += (int) _bgr_color.val[2];
            }
        }
        colorMsg.r = _sum_r / (recog_size * recog_size);
        colorMsg.g = _sum_g / (recog_size * recog_size);
        colorMsg.b = _sum_b / (recog_size * recog_size);
    }
}
void ColorRecogNode::publishImage()
{
    int _line_thickness = recog_size * 0.1;
    int _bg_thickness = _line_thickness + 2;

    // draw a rectangle bg(black)
    cv::rectangle(cvImgPtr->image,
            cv::Point(start_width - _line_thickness * 0.5, start_height - _line_thickness * 0.5),
            cv::Point(start_width + recog_size + _line_thickness, start_height + recog_size + _line_thickness),
            cv::Scalar(0, 0, 0),
            _bg_thickness);

    // draw a rectangle
    cv::rectangle(cvImgPtr->image,
            cv::Point(start_width - _line_thickness * 0.5, start_height - _line_thickness * 0.5),
            cv::Point(start_width + recog_size + _line_thickness, start_height + recog_size + _line_thickness),
            cv::Scalar(colorMsg.r, colorMsg.g, colorMsg.b),
            _line_thickness);

    // convert OpenCV image to ROS image and publish image
    imagePub.publish(cvImgPtr->toImageMsg());
}
void ColorRecogNode::publishColor()
{
    // publish OP_LED message
    colorPub.publish(colorMsg);
}
void ColorRecogNode::imageCallback(const sensor_msgs::Image::ConstPtr & msg)
{
    // convert ROS image to OpenCV image
    try
    {
        if ( msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ) this->imgEncoding = IMG_MONO;
        if ( msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ) this->imgEncoding = IMG_RGB8;
        this->cvImgPtr = cv_bridge::toCvCopy(msg, msg->encoding);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    image_height = msg->height;
    image_width = msg->width;
    recog_size = (int)(image_height * 0.1);
    start_height = (int)((image_height - recog_size) * 0.5);
    start_width = (int)((image_width - recog_size) * 0.5);

    // set new image flag
    this->newImageFlag = true;

    return;
}

Let's break down the above header file.

ColorRecogNode::ColorRecogNode() : nh(ros::this_node::getName()) , it(this->nh)
{
    //sets publishers
    imagePub = it.advertise("/image_out", 100);
    colorPub = nh.advertise<std_msgs::ColorRGBA>("/head_led_color", 100);      // for head led
    // colorPub = nh.advertise<std_msgs::ColorRGBA>("eye_led_color", 100);     // for eye leds

    //sets subscribers
    imageSubs = it.subscribe("/image_raw", 1, &ColorRecogNode::imageCallback, this);

    //initializes newImageFlag
    newImageFlag = false;
}

Tell the master that we are going to be publishing messages. This lets the master tell any nodes listening on /image_out or /head_led_color that we are going to publish data on that topic. The second argument is the size of our publishing queue. In this case if we are publishing too quickly it will buffer up a maximum of 100 messages before beginning to throw away old ones.

        for(int _row = start_height; _row < start_height + recog_size; _row++)
        {
            for(int _col = start_width; _col < (start_width + recog_size); _col ++)
            {
                // split rgb channel
                cv::Vec3b _bgr_color = cvImgPtr->image.at<cv::Vec3b>(_row, _col);

                _sum_r += (int) _bgr_color.val[0];
                _sum_g += (int) _bgr_color.val[1];
                _sum_b += (int) _bgr_color.val[2];
            }
        }

        colorMsg.r = _sum_r / (recog_size * recog_size);
        colorMsg.g = _sum_g / (recog_size * recog_size);
        colorMsg.b = _sum_b / (recog_size * recog_size);

Access specific channel of image Mat, Calculate average values of each channel.

    // convert OpenCV image to ROS image and publish image
    imagePub.publish(cvImgPtr->toImageMsg());

To convert a CvImage into a ROS image message, use one the toImageMsg() function. If the CvImage is one you have allocated youself, don't forget to fill in the header and encoding fields.

    // convert ROS image to OpenCV image
    try
    {
        if ( msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ) this->imgEncoding = IMG_MONO;
        if ( msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ) this->imgEncoding = IMG_RGB8;
        this->cvImgPtr = cv_bridge::toCvCopy(msg, msg->encoding);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

When converting a ROS sensor_msgs/Image message into a CvImage, CvBridge recognizes two distinct use cases:

  1. We want to modify the data in-place. We have to make a copy of the ROS message data. : toCvCopy
  2. We won't modify the data. We can safely share the data owned by the ROS message instead of copying. : toCvShare
Add a custom footer
Add a custom sidebar
Clone this wiki locally