Gazebo ImageStamped message to OpenCV

Convert Gazebo camera ImageStamped message to OpenCV Mat

Client code subscribe to gazebo image topic message type ImageStamped and view the image as opencv mat

• Terminal 1 (run gazebo)

make posix_sitl_default gazebo_typhoon_h480

• Termianl 2 (view topics and topic message)

gz topic -l
# 
/gazebo/default/typhoon_h480/camera/link/irlock
/gazebo/default/typhoon_h480/cgo3_camera_link/camera/cmd
/gazebo/default/typhoon_h480/cgo3_camera_link/camera/image
/gazebo/default/typhoon_h480/cgo3_camera_link/camera_imu/imu
/gazebo/default/typhoon_h480/cgo3_camera_link/wrench
/gazebo/default/typhoon_h480/cgo3_horizontal_arm_link/wr

#
gz topic -i /gazebo/default/typhoon_h480/cgo3_camera_link/camera/image
Type: gazebo.msgs.ImageStamped

• Terminal 2 (run the app)

cd build
cmake ..
make
../bin/viewer

• callback function

void cb(ConstImageStampedPtr &msg)
{
    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    //+1 for null terminate
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    //gazebo output rgb data
    //PixelFormat.R8G8B8
    cv::Mat image(height, width, CV_8UC3, data);

    cv::imshow("camera", image);
    cv::waitKey(1);
    delete data;  // DO NOT FORGET TO DELETE THIS, 
                  // ELSE GAZEBO WILL TAKE ALL YOUR MEMORY
}

cmake_minimum_required(VERSION 2.8)

project(gz_viewer)
set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)

find_package(gazebo REQUIRED)

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

find_package( OpenCV REQUIRED )
 
add_executable(viewer viewer.cpp)
target_link_libraries(viewer ${OpenCV_LIBS} ${GAZEBO_LIBRARIES} pthread)

Reference

• How to convert Gazebo's ConstImageStamped to cv::Mat?
• gz client standalone listener
• my source code

ROS Follow the line

Gazebo image topics

• Terminal1 (gazebo turtlebot)

roslaunch turtlebot_gazebo turtlebot_world.launch

• Terminal2

rostopic list

• depth camera
• rgb camera
  • raw: raw image
  • compress: compress image
  • theora: compress video

ROS sent image useing sensor_msgs/Image

Follower node

• subscribe to /camera/rgb/image_raw topic receive Image msg

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image

def image_callback(msg):
    pass

def main():
    rospy.init_node("follower")
    rospy.loginfo("start node")
    sub = rospy.Subscriber("/camera/rgb/image_raw", Image, image_callback)
    while not rospy.is_shutdown():
        rospy.spin()

if __name__ == "__main__":
    main()

• Set node permission (chmod +x)
• Run in separated Terminal

rosrun <package_name> follower.py

• Check that the node is running
• Get node info with rosnode info /follower
• Check message receive interval

rostopic hz /camera/rgb/image_raw

subscribed to [/camera/rgb/image_raw]
WARNING: may be using simulated time
average rate: 30.208
 min: 0.010s max: 0.060s std dev: 0.01642s window: 30
average rate: 28.061
 min: 0.010s max: 0.060s std dev: 0.01638s window: 56

Setup follow the line

launch file and projects structure

• launch file
  • worlds
• gazebo resource
  • materials

├── launch
│   └── follow.launch
├── materials
│   ├── course.material
│   └── course.png
├── models
├── scripts
│   └── follower.py
├── src
└── worlds
    └── course.world

<?xml version="1.0"?>
<launch>
    <env name="GAZEBO_RESOURCE_PATH" value="$(find follow_bot)/materials:$(optenv GAZEBO_RESOURCE_PATH)" />
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find follow_bot)/worlds/course.world"/>
    </include>

    <include file="$(find turtlebot_gazebo)/launch/includes/kobuki.launch.xml">
        <arg name="base" value="kobuki"/>
        <arg name="stacks" value="hexagons"/>
        <arg name="3d_sensor" value="kinect"/>
    </include>
</launch>

Note: launch file set GAZEBO_RESOURCE_PATH environment variable using env tag

View image

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
import cv2
import cv_bridge

class Follower(object):
    def __init__(self):
        self._bridge = cv_bridge.CvBridge()
        
        sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)

    def image_callback(self, msg):
        image = self._bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
        cv2.imshow("window", image)
        cv2.waitKey(3)


def on_shutdown():
    rospy.loginfo("shutdown")
    cv2.destroyAllWindows()

def main():
    rospy.init_node("follower")
    rospy.loginfo("start node")
    rospy.on_shutdown(on_shutdown)
    while not rospy.is_shutdown():
        follower = Follower()
        rospy.spin()

if __name__ == "__main__":
    main()

Note: When we use the cv2.namedWindow Method the Window was freeze check the line solution

Detecting the line

• Find the yellow line

lower_yellow = numpy.array([ 50, 50, 170])
upper_yellow = numpy.array([255, 255, 190])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

• left: orignal
• center: mask imaged (binary)
• right: masked image (extrat only the line from orignal image)

Find line center

• Mask image and take only relevant part (closer path front to the robot)

• Find line center with cv2 moments method
• For debug draw circle on line centroid

M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.circle(image, (cx, cy), 20, (0,0,255), -1)

- Public `Twist` message , calc z angular value
```python
#delta/ error between line center to image center
delta = cx - w/2
twist = Twist()
twist.linear.x = 0.2
twist.angular.z = -float(delta) / 100
self._cmd_vel_pub.publish(twist)

Final demo

Reference

• Programing robots with ROS book
• OpenCV moments
• My source code

ROS OpenCV hello

ros kinetic work with openCV 3.1

sudo apt install ros-kinetic-opencv3

• after install check with python

> import cv2
> cv2.__version__
'3.3.1-dev'

• Install helper utils and packages

sudo apt install ros-kinetic-usb-cam
sudo apt install ros-kinetic-image-view

CvBridge

Convert ROS image messages (sensor_msgs/Images) and OpenCv images (cv::Mat)

Demo

• Use as Gazebo camera plugin post as Image source
• Create package named cv_demo run the source code as rosnode cvnode.py package dependencies: sensor_msgs, opencv2, cv_bridge, rospy
• The node create OpenCv image window
• Use rqt to view publish image image_topic

Demo setup

• Terminal 1 (gazebo as image source)

roslaunch gazebo_ros empty_world.launch

• Terminal 2 (spawn camera)

rosrun gazebo_ros spawn_model -file camera.urdf -urdf -model bot

• Termianl 3 (run our node)

rosrun cv_demo cvnode.py

• Termianl 4 (rqt)

rqt

Reference

• Converting between ROS images and OpenCV images

Source code

#!/usr/bin/env python

import rospy
import cv2

from cv_bridge import CvBridge, CvBridgeError
import numpy as np
from sensor_msgs.msg import Image, CameraInfo

class CvBridgeDemo(object):
    NODE_NAME = "cv_demo"
    WIN_NAME = "cv_demo_win"

    def __init__(self):
        
        self._bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/bot/camera/image_raw", Image, self.image_callback)
        self.image_pub = rospy.Publisher("image_topic", Image)
        rospy.loginfo("watting for image")

    def image_callback(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        bridge = self._bridge
        try:
            frame = bridge.imgmsg_to_cv2(ros_image, "bgr8")
        except CvBridgeError, e:
            rospy.logerr("fail convert ros image to cv: {}".format(e))
        
        # Convert the image to a Numpy array for cv2 functions
        frame = np.array(frame, dtype=np.uint8)
        
        # Process the frame using the process_image() function
        display_image = self.process_image(frame)
                       
        # Display the image.
        cv2.imshow(CvBridgeDemo.WIN_NAME, display_image)
        
        # Process any keyboard commands
        cv2.waitKey(3)

        try:
            self.image_pub.publish(self._bridge.cv2_to_imgmsg(display_image, "mono8"))
        except CvBridgeError as e:
            rospy.logerr("fail convert cv image to rosmsg: {}".format(e))

    def process_image(self, frame):
        img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        font = cv2.FONT_HERSHEY_SIMPLEX
        cv2.putText(img,'OpenCV',(10,500), font, 4,(255,255,255),2,cv2.LINE_AA)
        return img

def on_shutdown():
    print "Shutting down cvnode."
    cv2.destroyAllWindows()

def main(): 
    rospy.init_node(CvBridgeDemo.NODE_NAME)      
    rospy.on_shutdown(on_shutdown)
    
    while not rospy.is_shutdown():
        CvBridgeDemo()
        rospy.loginfo("wait for shutdown")
        #The spin() code simply sleeps until the is_shutdown() flag is True. 
        # It is mainly used to prevent your Python Main thread from exiting.
        rospy.spin()
        

if __name__ == '__main__':
    main()