ROS by Example: Head Tracking using OpenCV

#!/usr/bin/env python

import roslib
roslib.load_manifest('pi_head_tracking_tutorial')
import sys
import rospy
import cv
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class test_vision_node:

    def __init__(self):
        rospy.init_node('test_vision_node')

        """ Give the OpenCV display window a name. """
        self.cv_window_name = "OpenCV Image"

        """ Create the window and make it re-sizeable (second parameter = 0) """
        cv.NamedWindow(self.cv_window_name, 0)

        """ Create the cv_bridge object """
        self.bridge = CvBridge()

        """ Subscribe to the raw camera image topic """
        self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.callback)

    def callback(self, data):
        try:
            """ Convert the raw image to OpenCV format """
            cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
        except CvBridgeError, e:
          print e
  
        
        """ Get the width and height of the image """
        (width, height) = cv.GetSize(cv_image)

        """ Overlay some text onto the image display """
        text_font = cv.InitFont(cv.CV_FONT_HERSHEY_DUPLEX, 2, 2)
        cv.PutText(cv_image, "OpenCV Image", (50, height / 2), text_font, cv.RGB(255, 255, 0))
  
        """ Refresh the image on the screen """
        cv.ShowImage(self.cv_window_name, cv_image)
        cv.WaitKey(3)

def main(args):
      vn = test_vision_node()
      try:
        rospy.spin()
      except KeyboardInterrupt:
        print "Shutting down vison node."
      cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)
    

#!/usr/bin/env python

""" camshift_node.py - Version 1.0 2010-12-28

    Modification of the ROS OpenCV Camshift example using cv_bridge and publishing the ROI
    coordinates to the /roi topic.   
"""

import roslib
roslib.load_manifest('pi_head_tracking_tutorial')
import sys
import rospy
import cv
from std_msgs.msg import String
from sensor_msgs.msg import Image, RegionOfInterest, CameraInfo
from cv_bridge import CvBridge, CvBridgeError

class vision_node:
    def __init__(self):
        rospy.init_node('vision_node')
        
        self.ROI = rospy.Publisher("roi", RegionOfInterest)
        
        """ Give the camera driver a moment to come up. """
        rospy.sleep(1)

        """ Create the display window """
        self.cv_window_name = "OpenCV Image"
        cv.NamedWindow(self.cv_window_name, 0)
        
        """ Create the cv_bridge object """
        self.bridge = CvBridge()
        
        """ Subscribe to the raw camera image topic """
        self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.callback)
        
        """ Set up a smaller window to display the CamShift histogram. """
        cv.NamedWindow("Histogram", 0)
        cv.MoveWindow("Histogram", 700, 10)
        cv.SetMouseCallback(self.cv_window_name, self.on_mouse)

        self.drag_start = None      # Set to (x,y) when mouse starts dragtime
        self.track_window = None    # Set to rect when the mouse drag finishes

        self.hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0,180)], 1 )
        self.backproject_mode = False

    def callback(self, data):
        """ Convert the raw image to OpenCV format using the convert_image() helper function """
        cv_image = self.convert_image(data)
        
        """ Apply the CamShift algorithm using the do_camshift() helper function """
        cv_image = self.do_camshift(cv_image)
        
        """ Refresh the displayed image """
        cv.ShowImage(self.cv_window_name, cv_image)
        
        """ Toggle between the normal and back projected image if user hits the 'b' key """
        c = cv.WaitKey(7) % 0x100
        if c == 27:
            return
        elif c == ord("b"):
            self.backproject_mode = not self.backproject_mode
          
    def convert_image(self, ros_image):
        try:
          cv_image = self.bridge.imgmsg_to_cv(ros_image, "bgr8")
          return cv_image
        except CvBridgeError, e:
          print e

    def do_camshift(self, cv_image):
        """ Get the image size """
        image_size = cv.GetSize(cv_image)
        image_width = image_size[0]
        image_height = image_size[1]
        
        """ Convert to HSV and keep the hue """
        hsv = cv.CreateImage(image_size, 8, 3)
        cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV)
        self.hue = cv.CreateImage(image_size, 8, 1)
        cv.Split(hsv, self.hue, None, None, None)

        """ Compute back projection """
        backproject = cv.CreateImage(image_size, 8, 1)

        """ Run the cam-shift algorithm """
        cv.CalcArrBackProject( [self.hue], backproject, self.hist )
        if self.track_window and is_rect_nonzero(self.track_window):
            crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
            (iters, (area, value, rect), track_box) = cv.CamShift(backproject,self.track_window, crit)
            self.track_window = rect

        """ If mouse is pressed, highlight the current selected rectangle
            and recompute the histogram """

        if self.drag_start and is_rect_nonzero(self.selection):
            sub = cv.GetSubRect(cv_image, self.selection)
            save = cv.CloneMat(sub)
            cv.ConvertScale(cv_image, cv_image, 0.5)
            cv.Copy(save, sub)
            x,y,w,h = self.selection
            cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255))

            sel = cv.GetSubRect(self.hue, self.selection )
            cv.CalcArrHist( [sel], self.hist, 0)
            (_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist)
            if max_val != 0:
                cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val)
        elif self.track_window and is_rect_nonzero(self.track_window):
            cv.EllipseBox( cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0 )
            
            roi = RegionOfInterest()
            roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2)))
            roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2)))
            roi.width = int(track_box[1][0])
            roi.height = int(track_box[1][1])
            self.ROI.publish(roi)

        cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist))
        
        if not self.backproject_mode:
            return cv_image
        else:
            return backproject
        

    def hue_histogram_as_image(self, hist):
            """ Returns a nice representation of a hue histogram """
    
            histimg_hsv = cv.CreateImage( (320,200), 8, 3)
            
            mybins = cv.CloneMatND(hist.bins)
            cv.Log(mybins, mybins)
            (_, hi, _, _) = cv.MinMaxLoc(mybins)
            cv.ConvertScale(mybins, mybins, 255. / hi)
    
            w,h = cv.GetSize(histimg_hsv)
            hdims = cv.GetDims(mybins)[0]
            for x in range(w):
                xh = (180 * x) / (w - 1)  # hue sweeps from 0-180 across the image
                val = int(mybins[int(hdims * x / w)] * h / 255)
                cv.Rectangle( histimg_hsv, (x, 0), (x, h-val), (xh,255,64), -1)
                cv.Rectangle( histimg_hsv, (x, h-val), (x, h), (xh,255,255), -1)
    
            histimg = cv.CreateImage( (320,200), 8, 3)
            cv.CvtColor(histimg_hsv, histimg, cv.CV_HSV2BGR)
            return histimg

    def on_mouse(self, event, x, y, flags, param):
        if event == cv.CV_EVENT_LBUTTONDOWN:
            self.drag_start = (x, y)
        if event == cv.CV_EVENT_LBUTTONUP:
            self.drag_start = None
            self.track_window = self.selection
        if self.drag_start:
            xmin = min(x, self.drag_start[0])
            ymin = min(y, self.drag_start[1])
            xmax = max(x, self.drag_start[0])
            ymax = max(y, self.drag_start[1])
            self.selection = (xmin, ymin, xmax - xmin, ymax - ymin)

def is_rect_nonzero(r):
    (_,_,w,h) = r
    return (w > 0) and (h > 0)           

def main(args):
      vn = vision_node()
      try:
        rospy.spin()
      except KeyboardInterrupt:
        print "Shutting down vision node."
      cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

#!/usr/bin/env python

"""
    head_track_node.py - Version 1.0 2010-12-28
    
    Move the head to track a target given by (x,y) coordinates
    
    Created for the Pi Robot Project: https://pirobot.org
    Copyright (c) 2010 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import roslib; roslib.load_manifest('pi_head_tracking_tutorial')
import rospy
from sensor_msgs.msg import JointState, RegionOfInterest, CameraInfo
import math
import time

class head_track():
    def __init__(self):
        rospy.init_node("head_track")
        rospy.on_shutdown(self.shutdown)
        
        """ Publish the movement commands on the /cmd_joints topic using the 
            JointState message type. """
        self.head_pub = rospy.Publisher("/cmd_joints", JointState)
        
        self.rate = rospy.get_param("~rate", 10)
        
        """ The pan/tilt thresholds indicate how many pixels the ROI needs to be off-center
            before we make a movement. """
        self.pan_threshold = int(rospy.get_param("~pan_threshold", 5))
        self.tilt_threshold = int(rospy.get_param("~tilt_threshold", 5))
        
        """ The k_pan and k_tilt parameter determine how responsive the servo movements are.
            If these are set too high, oscillation can result. """
        self.k_pan = rospy.get_param("~k_pan", 7.0)
        self.k_tilt = rospy.get_param("~k_tilt", 5.0)

        
        self.max_pan = rospy.get_param("~max_pan", math.radians(90))
        self.min_pan = rospy.get_param("~min_pan", math.radians(-90))
        self.max_tilt = rospy.get_param("~max_tilt", math.radians(45))
        self.min_tilt = rospy.get_param("~min_tilt", math.radians(-45))
        
        r = rospy.Rate(self.rate) 
        
        self.head_cmd = JointState()
        self.joints = ["head_pan_joint", "head_tilt_joint"]
        self.head_cmd.name = self.joints
        self.head_cmd.position = [0, 0]
        self.head_cmd.velocity = [1, 1]
        self.head_cmd.header.stamp = rospy.Time.now()
        self.head_cmd.header.frame_id = 'head_pan_joint'
    
        """ Center the head and pan servos at the start. """
        for i in range(3):
            self.head_pub.publish(self.head_cmd)
            rospy.sleep(1)
        
        self.tracking_seq = 0
        self.last_tracking_seq = -1
        
        rospy.Subscriber('roi', RegionOfInterest, self.setPanTiltSpeeds)
        rospy.Subscriber('/camera/camera_info', CameraInfo, self.getCameraInfo)

        while not rospy.is_shutdown():
            """ Publish the pan/tilt movement commands. """
            self.head_cmd.header.stamp = rospy.Time.now()
            self.head_cmd.header.frame_id = 'head_pan_joint'
            if self.last_tracking_seq == self.tracking_seq:
                self.head_cmd.velocity = [0.0001, 0.0001]
            else:
                self.last_tracking_seq = self.tracking_seq
            self.head_pub.publish(self.head_cmd)
            r.sleep()

    def setPanTiltSpeeds(self, msg):
        """ When OpenCV loses the ROI, the message stops updating.  Use this counter to
            determine when it stops. """
        self.tracking_seq += 1
        
        """ Check to see if we have lost the ROI. """
        if msg.width == 0 or msg.height == 0 or msg.width > self.image_width / 2 or \
          msg.height > self.image_height / 2:
            self.head_cmd.velocity = [0.0001, 0.0001]
            return

        """ Compute the center of the ROI """
        COG_x = msg.x_offset + msg.width / 2 - self.image_width / 2
        COG_y = msg.y_offset + msg.height / 2 - self.image_height / 2
          
        """ Pan the camera only if the displacement of the COG exceeds the threshold. """
        if abs(COG_x) > self.pan_threshold:
            """ Set the pan speed proportion to the displacement of the horizontal displacement
                of the target. """
            self.head_cmd.velocity[0] = self.k_pan * abs(COG_x) / float(self.image_width)
               
            """ Set the target position to one of the min or max positions--we'll never
                get there since we are tracking using speed. """
            if COG_x > 0:
                self.head_cmd.position[0] = self.min_pan
            else:
                self.head_cmd.position[0] = self.max_pan
        else:
            self.head_cmd.velocity[0] = 0.0001
        
        """ Tilt the camera only if the displacement of the COG exceeds the threshold. """
        if abs(COG_y) > self.tilt_threshold:
            """ Set the tilt speed proportion to the displacement of the vertical displacement
                of the target. """
            self.head_cmd.velocity[1] = self.k_tilt * abs(COG_y) / float(self.image_height)
            
            """ Set the target position to one of the min or max positions--we'll never
                get there since we are tracking using speed. """
            if COG_y < 0:
                self.head_cmd.position[1] = self.min_tilt
            else:
                self.head_cmd.position[1] = self.max_tilt
        else:
            self.head_cmd.velocity[1] = 0.0001
            
    def getCameraInfo(self, msg):
        self.image_width = msg.width
        self.image_height = msg.height
        
    def shutdown(self):
        rospy.loginfo("Shutting down head tracking node...")         
                   
if __name__ == '__main__':
    try:
        head_track()
    except rospy.ROSInterruptException:
        rospy.loginfo("Head tracking node is shut down.")

#!/usr/bin/env python

"""
    robotis_joint_controller.py - Version 1.0 2010-12-28
    
    Joint Controller for AX-12 servos on a USB2Dynamixel device using
    the Robotis Controller Package from the Healthcare Robotics Lab at Georgia Tech
    
    Created for the Pi Robot Project: https://pirobot.org
    Copyright (c) 2010 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import roslib; roslib.load_manifest('robotis')
import rospy
import servo_config as sc
from robotis.lib_robotis import Robotis_Servo, USB2Dynamixel_Device
from robotis.ros_robotis import ROS_Robotis_Server, ROS_Robotis_Client
from sensor_msgs.msg import JointState
        
class joint_controller():
    def __init__(self):
        rospy.init_node('joint_controller')
        
        rospy.on_shutdown(self.shutdown)
        
        joint_state_pub = rospy.Publisher('/joint_states', JointState)
        
        self.port = rospy.get_param('~port', '/dev/ttyUSB0')
        self.rate = rospy.get_param('~rate', 10)
        r = rospy.Rate(self.rate)
        
        """ Read in the servo_config.py file. """
        self.servo_param = sc.servo_param

        self.controllers = dict()
        self.joints = dict()
        self.ids = list()
        
        """ Read servo ids and joint names from the servo config file. """
        for id in self.servo_param.keys():
            joint = self.servo_param[id]['name']
            self.joints[id] = joint
            self.ids.append(id)

        """ Connect to the USB2Dynamixel controller """
        usb2dynamixel = USB2Dynamixel_Device(self.port)

        """ Fire up the ROS joint server processes. """
        servos = [ Robotis_Servo( usb2dynamixel, i ) for i in self.ids ]
        ros_servers = [ ROS_Robotis_Server( s, str(j) ) for s, j in zip(servos, self.joints.values()) ]
        
        """ Fire up the ROS services. """
        for joint in self.joints.values():
            self.controllers[joint] = ROS_Robotis_Client(joint)
        
        """ Start the joint command subscriber """
        rospy.Subscriber('cmd_joints', JointState, self.cmd_joints_handler) 
        
        while not rospy.is_shutdown():
            """ Create a JointState object which we use to publish the current state of the joints. """
            joint_state = JointState()
            joint_state.name = list()
            joint_state.position = list()
            joint_state.velocity = list()
            joint_state.effort = list()
            for s in ros_servers:
                joint_state.name.append(s.name)
                joint_state.position.append(s.update_server())
                """ The robotis driver does not currently query the speed and torque of the servos,
                    so just set them to 0. """
                joint_state.velocity.append(0)
                joint_state.effort.append(0)
            joint_state.header.stamp = rospy.Time.now()
            joint_state_pub.publish(joint_state)
            r.sleep()
            
    def cmd_joints_handler(self, req):
        for joint in self.joints.values():
            try:
                self.controllers[joint].move_angle(req.position[req.name.index(joint)], 
                max(0.01, req.velocity[req.name.index(joint)]), blocking=False)
            except:
                pass
            
    def shutdown(self):
        rospy.loginfo('Shutting down joint command controller node...')

            
if __name__ == '__main__':
    try:
        jc = joint_controller()
    except rospy.ROSInterruptException:
        pass