A Brief History of the Pi Robot Project

The Early Days

Pi Robot Meets ROS

ROS has made its biggest impact in university robotics labs. For this reason, the framework might appear beyond the reach of the typical hobby roboticist.  To be sure, the ROS learning curve is a little steep and a complete beginner might find it somewhat intimidating.  For one thing, the full ROS framework only runs under Linux at the moment, though parts of it can be made to work under Windows or other OSes. 

So you'll need a Linux installation (preferably Ubuntu, and it's free) or a Linux alongside your existing OS.  (Ubuntu can even be installed under Windows as just another application without the need for repartitioning.)  Once you have Linux up and running, you can turn to the ROS Wiki for installation instructions and a set of excellent beginner tutorials for both Python and C++.  It is very important to actually work through the examples on your own computer, not just read through the text.  With practise, comes familiarity and then if you're like me, ROS will really start to grow on you.

ROS can now be used with many hobby-level robots and robot controllers including the Lego NXT, Rovio, iCreate, ArbotiX, Serializer, Dynamixels, Phidgets and more.  Even though you can program these robots many other ways, if you are interested in something as complex as SLAM, you might well take a look at ROS.  In the end, the time put into learning ROS amounts to a tiny fraction of what it would take to develop all the code from scratch.  For example, suppose you want to program your robot to take an object from your location in the dining room to somebody else in the bedroom, all while avoiding obstacles.  You can certainly solve this problem yourself using visual landmarks or some kind of beacon system.  But whole books have been written on localization and navigation by some of the best roboticists in the world, so why not capitalize on their efforts?  ROS allows you to do precisely this by plugging your robot directly into the pre-existing navigation stack, a set of routines designed to map sensor data and odometery information from your robot into motion commands and automatic localization.  All you need to provide are the dimensions of your robot plus the sensor and encoder data and away you go.  The hundreds or thousands of hours you just saved by not reinventing the wheel can now be spent on something else such as having your robot tidy your room or fold the laundry.

ROS Highlights

RViz is typically used to display sensor readings and the layout of obstacles such as walls and other objects.  In particular, it is good at displaying the data returned from laser range finders (also called LIDAR) and stereo vision (point clouds).  There is even a new kind of low cost vision sensor, the Microsoft Kinect, that can produce a 3D point cloud using Both of these data types consist of points located at various distances from the robot.  A laser scanner returns an array of distance measurements lying in a single plane and typically sweeps through an arc in front of and to the sides of the robot.  A stereo camera returns a set of distances (disparity measures) across the planar field of view of the two cameras.  In any event, these distance readings can be visualized in RViz as a set of points as illustrated by the orange spheres in the images on the right.  In the first image, the readings are all equidistant from the scanner as though there were a long sheet of cardboard bent in an arc in front of the robot.  In the second image, Pi is standing in front of a hallway so that the distance readings recede as the scanner probes through the opening.  Both of these images where created using an inexpensive substitute for a laser scanner that anyone can make and is described in greater detail below.

Navigation and Obstacle Avoidance

The ROS navigation system enables a robot to move from point A to B without running into things.  To do true SLAM (simultaneous localization and mapping), your robot will generally need a laser range finder or the Kinect IR-depth camera from Microsoft.  If you have a good stereo camera you could also try visual SLAM or VSLAM.  However, basic obstacle avoidance and navigation by dead reckoning can be accomplished with an inexpensive alternative dubbed the "Poor Man's Lidar" or PML, a concept originated by Bob Mottram and perfected by Mike Ferguson using the ArbotiX controller.

A PML consists of a low cost IR sensor mounted on a panning servo that continually sweeps the sensor through an arc in front of the robot. The servo-plus-sensor can record 30 readings per 180-degree sweep which takes 1 second in each direction.  As a result, there is a bit of a lag between the motion of the robot and the updated range readings indicated by the orange balls in the images above and the videos below.  By comparison, the lowest cost laser range finder (about $1300)  takes over 600 distance readings per 240-degree sweep and covers the entire arc 10 times per second (1/10th of a second per sweep).

The photos below show our PML setup:

Toward the bottom of the first photo you can see the IR sensor (Sharp model GP2Y0A02YK) attached to a Robotis Dynamixel AX-12+ servo.  Notice how the IR sensor is mounted "vertically" which is a better orientation when taking sweeping horizontal measurements.  The second photo better illustrates how the IR sensor sweeps to one side, and the third photo shows the ArbotiX microcontroller attached to the back of Pi's torso.  The IR sensor plugs into one of the ArbotiX analog sensor ports while the AX-12 servo plugs into the Dynamixel bus.  The ArbotiX firmware includes direct support for a PML sensor of this type and Vanadium labs has developed an open source ROS node that allows the PML data to appear as a "laser scan" within the ROS framework and RViz.  The image below shows the PML data from a single scan while Pi stands in front of a box which itself is in front of a wall.

Our PML has a rather limited range compared to a laser scanner. The Sharp GP2Y0A02YK can measure distances between 20 cm (0.2 meters) and 1.5 meters.  A typical laser scanner has a range between 2 cm (0.02 meters) to 5.5 meters.   Longer range IR sensors are available such as the GP2Y0A700K0F which measures between 1.0 and 5.5 meters but this means the robot would be blind to objects within 1 meter.   We could also use a pair of short and long range sensors, but for this article we'll use just a single sensor.  

The first video shows Pi moving around a room while avoiding obstacles using a PML.  The grid squares in RViz are 0.25 meters on a side (about 10 inches) and you can see that the user clicks on a location with the mouse to tell Pi where to go next.  (The green arrow indicates the orientation we want the robot to have once it gets to the target location.)  ROS then figures out the best path to follow to get there (indicated by the faint green line) and incorporates the data points from the PML scanner to avoid obstacles along the way.  When an obstacle is detected, a red square is placed on the map to indicate that the cell is occupied. The grey squares add a little insurance based on the dimensions of Pi's base just to make sure we don't get caught on an edge.  Be sure to view the video in full screen mode by clicking on the little box with 4 arrows at the bottom right corner of the video.

This video demonstrates the use of the ROS navigation stack with a "Poor Man's Lidar" consisting of a low cost Sharp IR sensor (model GP2Y0A02YK) with a panning servo (Dynamixel AX-12+) and the ArbotiX microcontroller by Vanadium Labs.  Odometry data is obtained from a Serializer microcontroller made by the RoboticsConnection and connected to a pair of 7.2V gearhead drive motors equipped with integrated encoders (624 counts per revolution).  Communication to the controlling PC is by way of XBee for the ArbotiX and Bluetooth for the Serializer.

Robot Cartography: ROS + SLAM

ROS by Example: Visual Object Tracking

• Install Ubuntu Linux (I am using version 10.04 on a machine that dual boots with Windows).  If your robot does not have its own onboard computer, you can still run the code on your main computer and connect your robot's video camera and servo controller appropriately (usually via USB ports).
  
• Install the ROS C Turtle release on your Ubuntu machine.
  
• If you are not already familiar with the ROS basics, work through the Beginner Tutorials.  It is important to actually try the sample code on your own machine rather than just reading through the text.  In fact, I ran through all the tutorials twice since a few concepts were a little shaky after a single pass.
  

ROS Recap

The core entity in ROS is called a node. A node is generally a small program written in Python or C++ that executes some relatively simple task or process. Nodes can be started and stopped independently of one another and they communicate by passing messages. A node can publish messages on certain topics or provide services to other nodes.

For example, a publisher node might report data from sensors attached to your robot's microcontroller. A message on the /head_sonar topic with a value of 0.5 would mean that the sensor is currently detecting an object 0.5 meters away. (Note that ROS uses meters for distance and radians for angular measurements.)  Any node that wants to know the reading from this sensor need only subscribe to the /head_sonar topic. To make use of these values, the subscriber node defines a callback function that gets executed whenever the message on the subscribed topic is updated. How often this happens depends on the rate at which the publisher node updates its message(s).

A node can also define one or more services that produce an action or send back a reply when sent a request from another node. A good example is a service that talks to a servo controller. In this case, a service request would consist of a message specifying the servo id as well as its goal position, velocity and/or effort (torque). The reply message could be null or it could simply acknowledge receipt of the message. The controller would then move the servo in the desired manner.

More complex nodes will subscribe to a number of topics and services, combine the results in a useful way, and perhaps publish messages or provide services of their own.  For example, the head tracking node we will develop below subscribes to camera messages on a set of video topics and publishes movement commands on another topic that are then read by a servo controller node to move the head's pan and tilt servos.

Vision with OpenCV

Servo Control

Setting Up the ROS Nodes

• The camera node: obtains a video stream from the camera.  This is our low level driver to the camera.
  
• The visual perception node: extracts a set of pixels defining the object we want to track and publishes the coordinates of this region of interest (ROI) on the topic /roi.
  
• The head tracking node: subscribes to the /roi topic and computes movement commands that keep the target centered in the field of view.  The command are published on the /cmd_joints topic.
  
• The joint controller node: subscribes to the /cmd_joints topic and maps movement commands into actual servo motions for the pan and tilt motors.

$ roscreate-pkg head_tracking_tutorial
$ cd head_tracking_tutorial
$ mkdir bin launch params

 
     Head Tracking Tutorial
 
  Patrick Goebel
  BSD
 
  https://pirobot.org/blog/0016
 
 
 
 
 
 
 
 
 

Installing the uvc_cam Package

$ git clone https://github.com/ericperko/uvc_cam.git

$ rosmake --rosdep-install uvc_cam

Installing the ArbotiX Package

$ svn checkout http://vanadium-ros-pkg.googlecode.com/svn/trunk/vanadium_drivers/arbotix

$ cd arbotix
$ svn checkout http://pi-robot-ros-pkg.googlecode.com/svn/trunk/pi_sandbox/pi_arbotix_patch

$ patch -p1 < pi_arbotix_patch/arbotix.patch

$ rosmake --rosdep-install arbotix

Installing the Robotis Package

$ svn checkout http://gt-ros-pkg.googlecode.com/svn/trunk/hrl/robotis/

$ rosmake --rosdep-install robotis

Building the Head Tracking Package

$ rosmake --rosdep-install

The Camera Node

$ roslaunch head_tracking_tutorial camera.launch

$ rosrun image_view image_view image:=/camera/image_raw

$ roscd head_tracking_tutorial/params
$ rosrun dynamic_reconfigure dynparam dump /uvc_cam_node philips_spc1300.yaml

$ roslaunch head_tracking_tutorial camera.launch
$ rosrun image_view image_view image:=/camera/image_raw

$ sudo apt-get install guvcview

$ guvcview

The Visual Perception Node

#!/usr/bin/env python

import roslib
roslib.load_manifest('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)
    

$ chmod 755 test_vision_node.py

$ rosrun head_tracking_tutorial test_vision_node.py

Adding Visual Filters

#!/usr/bin/env python

""" vision_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('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)

The Head Tracking Node

#!/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('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, 0]
            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, 0]
            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.0
        
        """ 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.0
            
    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.")

$ roslaunch head_tracking_tutorial head_track.launch

$ rostopic echo /cmd_joints

$ rxplot /cmd_joints/velocity[0]:velocity[1]

The Joint Controller Node: USB2Dynamixel and the Robotis ROS Package

#!/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

from math import radians
servo_param = {
    1: {'name': 'head_pan_joint',
        'home_encoder': 512,
        'max_speed': radians(180),
        'max_ang': radians(145.),
        'min_ang': radians(-145.)
       },
    2: {'name': 'head_tilt_joint',
        'home_encoder': 512,
        'max_speed': radians(180),
        'max_ang': radians(90.),
        'min_ang': radians(-90.)
       }
}

$ roscd robotis/src/robotis
$ roscp head_tracking_tutorial servo_config.py .

$ roslaunch head_tracking_tutorial robotis.launch

$ rostopic pub /cmd_joints sensor_msgs/JointState '{ header: {frame_id: test }, name: ['head_pan_joint'], position: [1], velocity: [1], effort: [1]}'

The Joint Controller Node: ArbotiX and the Vanadium Labs ROS Package

$ svn checkout http://vanadium-ros-pkg.googlecode.com/svn/trunk/vanadium_drivers/arbotix

$ rosmake --rosdep-install arbotix

port: /dev/ttyUSB0
baud: 57600
rate: 10
use_sync: True

dynamixels: {
    head_pan_joint: {id: 1, max_speed: 180, min_angle: -145, max_angle: 145},
    head_tilt_joint: {id: 2, max_speed: 180, min_angle: -90, max_angle: 90}
}

controllers: {
    joint_controller: {type: joint_controller, joints: [head_pan_joint, head_tilt_joint] }
}

$ roslaunch head_tracking_tutorial arbotix.launch

$ rostopic pub /cmd_joints sensor_msgs/JointState '{ header: {frame_id: test }, name: ['head_pan_joint'], position: [1], velocity: [1], effort: [1]}'

Putting it all Together

$ roslaunch head_tracking_tutorial head_track.launch

Adding RViz for Visualization (Bonus!)