ohmm
Class CvVisualServo

java.lang.Object
  ohmm.CvBase
      ohmm.CvUndistort
          ohmm.CvVisualServo

public class CvVisualServo
extends CvUndistort

Visual servoing to grasp a colored object (CS4610 LAB4 solution).

Uses CvBase for imaging and Grasp for arm control, and uses ImageServer to serve debug images.

Basic operation proceeds through the following series of states:

• RESET - (return to) setup pose
• INIT - wait for user to click on image
• CAL - determine calibration color on next frame
• WAIT - wait for runPending user to hit 'r'
• ORIENT - visual servo orientation
• DRIVE - visual servo fwd/reverse
• GRASP - arm kinematic sequence to acquire object
• HOMING - drive to orig pose (if enableHoming)
• DONE - quits main loop

The calibration color is determined as the average over a square of radius calRad pixels about the clicked pixel. The track point is then initialized as the calibration click point.

At every state after CAL the image is processed to look for an object with the calibration color. If found, its center coordinates are saved in trackX, trackY. The image processing steps are:

1. convert from BGR to HSV
2. gaussian smooth
3. threshold in HSV space
4. erode and dilate openIt iters ea (morphological opening)
5. find countours
6. find the contour w/ bbox ctr C1 closest to the track pt, if any
7. find the contour w/ bbox is largest with bbox ctr C2
8. set new track pt as C1 if possible, else C2

Visual servoing is two-level bang-bang control first in yaw (orientation), then in radial drive position. Orientation and drive servo phases cycle until the goal is within tol. If the object is initially too low in the frame (i.e. too close to the robot) then the first yaw phase is skipped to avoid collision with the object.

Action does not begin until calling CvBase.mainLoop(), which then blocks until DONE. Many instance fields, including state, can be customized; these are declared public volatile so they may be changed in threads other than the one running CvBase.mainLoop() (more advanced syncronization is not needed here because the possibility to introduce inconsistencies across fields is low and the potential effects of any such inconsistency should be benign). Handling of each state is also broken out into separate protected functions that could be overridden in subclasses.

The alternate constructor CvVisualServo(Grasp, int, int, int) is one convenient way to skip the interactive calibration states and also the final homing behavior.

Author:

Marsette Vona

Field Summary

static java.lang.String	APPNAME Application name.
protected java.lang.Thread	armThread Thread for running arm motions.
int	ballDiameter Ball diameter in world frame mm, see estimateBall().
int	ballHeight Ball Z coord in world frame mm, see estimateBall().
protected com.googlecode.javacv.cpp.opencv_core.IplImage	binAltImg Work images.
protected com.googlecode.javacv.cpp.opencv_core.IplImage	binImg Work images.
static int	CAL Visual servo state.
int	calB Calibration pixel.
int	calG Calibration pixel.
int	calH Calibration pixel.
int	calMin Calibration gather box radius and min pixels.
int	calR Calibration pixel.
int	calRad Calibration gather box radius and min pixels.
int	calS Calibration pixel.
int	calV Calibration pixel.
int	calX Calibration pixel.
int	calY Calibration pixel.
protected com.googlecode.javacv.cpp.opencv_core.CvSeq	contour First contour in sequence.
static int	DBG_R Debug + radius in pixels.
protected com.googlecode.javacv.cpp.opencv_core.IplImage	dbgAltImg Work images.
protected int	dbgFrameCountdown Countdown to a debug frame.
int	dbgFreq Debug image frequency relative to process(com.googlecode.javacv.cpp.opencv_core.IplImage) frequency.
int	dbgH Debug image dims.
protected com.googlecode.javacv.cpp.opencv_core.IplImage	dbgImg Work images.
int	dbgW Debug image dims.
static int	DEF_BALL_DIAMETER Default ballDiameter.
static int	DEF_BALL_HEIGHT Default ballHeight.
static int	DEF_CAL_MIN Default calRad, calMin.
static int	DEF_CAL_RAD Default calRad, calMin.
static int	DEF_CAM Default camera index.
static int	DEF_DBG_FREQ Default dbgFreq.
static int	DEF_DBG_H Default dbgW, dbgH.
static int	DEF_DBG_W Default dbgW, dbgH.
static int	DEF_DRIVE_DONE_THRESH Default servo state pixel thresholds.
static float	DEF_DRIVE_FAST Default straight drive speeds in mm/sec.
static float	DEF_DRIVE_SLOW Default straight drive speeds in mm/sec.
static int	DEF_DRIVE_SLOW_THRESH Default servo state pixel thresholds.
static float	DEF_EXTEND_X Default EE location in robot frame mm at retract/reach pose.
static float	DEF_EXTEND_Z Default EE location in robot frame mm at retract/reach pose.
static int	DEF_H_TOL Default visual servo default threshold parameters.
static int	DEF_IMG_H Default image size; must be set in call to CvBase.init(int, java.lang.String[]).
static int	DEF_IMG_W Default image size; must be set in call to CvBase.init(int, java.lang.String[]).
static int	DEF_MAX_DIFF_TRACK Default maxDiffTrack.
static double	DEF_MIN_SERVO_TIME Default minServoTime.
static int	DEF_OPEN_IT Default visual servo morphological open iterations.
static int	DEF_ORIENT_DONE_THRESH Default servo state pixel thresholds.
static float	DEF_ORIENT_FAST Default turn in place speeds in rad/sec.
static float	DEF_ORIENT_SLOW Default turn in place speeds in rad/sec.
static int	DEF_ORIENT_SLOW_THRESH Default servo state pixel thresholds.
static float	DEF_RETRACT_X Default EE location in robot frame mm at retract/reach pose.
static float	DEF_RETRACT_Z Default EE location in robot frame mm at retract/reach pose.
static int	DEF_S_MAX Default visual servo default threshold parameters.
static int	DEF_S_MIN Default visual servo default threshold parameters.
static float	DEF_T0_UP Default joints in rad in the visual servo "up" pose.
static float	DEF_T1_UP Default joints in rad in the visual servo "up" pose.
static float	DEF_T2_UP Default joints in rad in the visual servo "up" pose.
static int	DEF_TRACK_D_FUDGE Default trackDFudge.
static int	DEF_V_MAX Default visual servo default threshold parameters.
static int	DEF_V_MIN Default visual servo default threshold parameters.
static int	DONE Visual servo state.
static int	DRIVE Visual servo state.
int	driveDoneThresh Servo state pixel thresholds.
float	driveFast Straight drive speeds in mm/sec.
float	driveSlow Straight drive speeds in mm/sec.
int	driveSlowThresh Servo state pixel thresholds.
boolean	enableHoming Whether to enable the HOMING state.
float	extendX EE location in robot frame mm at reach pose.
float	extendZ EE location in robot frame mm at reach pose.
Grasp	grasp The Grasp object.
static int	GRASP Visual servo state.
int	hAltMax Threshold parameters.
int	hAltMin Threshold parameters.
int	hMax Threshold parameters.
int	hMin Threshold parameters.
static int	HOMING Visual servo state.
com.googlecode.javacv.cpp.opencv_core.CvScalar	hsvAltMax HSV threshold bounds.
com.googlecode.javacv.cpp.opencv_core.CvScalar	hsvAltMin HSV threshold bounds.
protected com.googlecode.javacv.cpp.opencv_core.IplImage	hsvImg Work images.
com.googlecode.javacv.cpp.opencv_core.CvScalar	hsvMax HSV threshold bounds.
com.googlecode.javacv.cpp.opencv_core.CvScalar	hsvMin HSV threshold bounds.
int	hTol Threshold parameters.
protected int	imgH Image dims, detected on first call to process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected int	imgW Image dims, detected on first call to process(com.googlecode.javacv.cpp.opencv_core.IplImage).
static int	INIT Visual servo state.
protected float[]	initXYT Initial robot pose.
protected double	lastServoTime Time of last servo update.
int	maxDiffTrack Maximum change in track point in pixels.
double	minServoTime Minimum time between servo updates in seconds.
int	mouseX Mouse location in image or (-1, -1) if none.
int	mouseY Mouse location in image or (-1, -1) if none.
OHMMDrive	ohmm The OHMMDrive object.
int	openIt Morphological open iterations.
static int	ORIENT Visual servo state.
int	orientDoneThresh Servo state pixel thresholds.
float	orientFast Turn in place speeds in rad/sec.
float	orientSlow Turn in place speeds in rad/sec.
int	orientSlowThresh Servo state pixel thresholds.
static int	RESET Visual servo state.
float	retractX EE location in robot frame mm at retract pose.
float	retractZ EE location in robot frame mm at retract pose.
boolean	runPending Whether the ORIENT state is pending.
static java.lang.String	RXTX_PORT RXTX port filename for test driver only.
int	sMax Threshold parameters.
int	sMin Threshold parameters.
int	state State machine state, see class header doc.
protected com.googlecode.javacv.cpp.opencv_core.CvMemStorage	storage For storing contours.
float	t0Up Arm joints in rad in the visual servo "up" pose.
float	t1Up Arm joints in rad in the visual servo "up" pose.
float	t2Up Arm joints in rad in the visual servo "up" pose.
int	trackD Tracked blob diameter or -1 if no tracking, see estimateBall().
int	trackDFudge Fudge factor for trackD.
int	trackX Tracked pixel or (-1, -1) for no tracking.
int	trackY Tracked pixel or (-1, -1) for no tracking.
int	vMax Threshold parameters.
int	vMin Threshold parameters.
static int	WAIT Visual servo state.

Fields inherited from class ohmm.CvUndistort

distortions, extrinsics, intrinsics, mapx, mapy, undistort, undistortImage

Fields inherited from class ohmm.CvBase

appname, brightness, canvasFrame, canvasFrameKeyEvent, cap, CAP_PROP_NAMES, CAP_PROPS, capImage, dbg, DEF_APPNAME, DEF_CAM_H, DEF_CAM_W, DEF_INPUT, DEF_MAX_FPS, DEF_MIN_DELAY_MS, DEF_SERVER_FPS, DEF_SERVER_PORT, DEF_USE_CANVAS_FRAME, DEF_USE_CONSOLE, DEF_USE_SERVER, DEF_USE_WINDOW, enableExit, exposure, FMT, frameEndMS, frameN, gain, grabber, height, hue, maxFPS, minDelayMS, mouseCallback, msgStream, msPerTick, paused, procImage, saturation, saveImage, server, serverFPS, serverImage, serverKey, serverPort, streamIndex, useCanvasFrame, useConsole, useServer, useWindow, V4L2_AUTO_PROP_NAMES, V4L2_AUTO_PROP_OFF_VALS, V4L2_AUTO_PROP_ON_VALS, V4L2_AUTO_PROPS, V4L2_CID_AUDIO_BALANCE, V4L2_CID_AUDIO_BASS, V4L2_CID_AUDIO_LOUDNESS, V4L2_CID_AUDIO_MUTE, V4L2_CID_AUDIO_TREBLE, V4L2_CID_AUDIO_VOLUME, V4L2_CID_AUTO_WHITE_BALANCE, V4L2_CID_AUTOGAIN, V4L2_CID_BACKLIGHT_COMPENSATION, V4L2_CID_BASE, V4L2_CID_BLACK_LEVEL, V4L2_CID_BLUE_BALANCE, V4L2_CID_BRIGHTNESS, V4L2_CID_CAMERA_CLASS, V4L2_CID_CAMERA_CLASS_BASE, V4L2_CID_CONTRAST, V4L2_CID_DO_WHITE_BALANCE, V4L2_CID_EXPOSURE, V4L2_CID_EXPOSURE_ABSOLUTE, V4L2_CID_EXPOSURE_AUTO, V4L2_CID_EXPOSURE_AUTO_PRIORITY, V4L2_CID_FOCUS_ABSOLUTE, V4L2_CID_FOCUS_AUTO, V4L2_CID_FOCUS_RELATIVE, V4L2_CID_GAIN, V4L2_CID_GAMMA, V4L2_CID_HCENTER_DEPRECATED, V4L2_CID_HFLIP, V4L2_CID_HUE, V4L2_CID_HUE_AUTO, V4L2_CID_LASTP1, V4L2_CID_PAN_ABSOLUTE, V4L2_CID_PAN_RELATIVE, V4L2_CID_PAN_RESET, V4L2_CID_POWER_LINE_FREQUENCY, V4L2_CID_POWER_LINE_FREQUENCY_50HZ, V4L2_CID_POWER_LINE_FREQUENCY_60HZ, V4L2_CID_POWER_LINE_FREQUENCY_DISABLED, V4L2_CID_PRIVATE_BASE, V4L2_CID_RED_BALANCE, V4L2_CID_SATURATION, V4L2_CID_SHARPNESS, V4L2_CID_TILT_ABSOLUTE, V4L2_CID_TILT_RELATIVE, V4L2_CID_TILT_RESET, V4L2_CID_USER_BASE, V4L2_CID_USER_CLASS, V4L2_CID_VCENTER_DEPRECATED, V4L2_CID_VFLIP, V4L2_CID_WHITE_BALANCE_TEMPERATURE, V4L2_CID_WHITENESS, V4L2_CTRL_CLASS_CAMERA, V4L2_CTRL_CLASS_MPEG, V4L2_CTRL_CLASS_USER, V4L2_EXPOSURE_APERTURE_PRIORITY, V4L2_EXPOSURE_AUTO, V4L2_EXPOSURE_MANUAL, V4L2_EXPOSURE_SHUTTER_PRIORITY, v4l2Auto, warnStream, width

Constructor Summary

CvVisualServo(Grasp grasp)
Inits frame processing.

CvVisualServo(Grasp grasp, int calH, int calX, int calY)
Convenience constructor to run non-interactive.

Method Summary

void	abort() reset() and set state DONE
protected boolean	doneProcessing() This impl checks if state is DONE.
protected void	estimateBall() Estimate ball coordinates in robot frame (optional).
protected void	guiHelpExt() Show help for custom keypresses.
protected boolean	handleKeyExt(int code) Handle custom keypresses.
protected void	handleMouse(int event, int x, int y, int flags) Handle mouse clicks.
static void	main(java.lang.String[] argv) Test driver.
protected com.googlecode.javacv.cpp.opencv_core.IplImage	process(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Do all the frame processing and servoing.
protected void	processCal(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processDrive(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processGrasp(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processHoming(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processOrient(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processReset(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected com.googlecode.javacv.cpp.opencv_core.IplImage	processUpdateReturnImge(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Create/update the return image for process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processUpdateTracking(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Update trackX, trackY in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
protected void	processWait(com.googlecode.javacv.cpp.opencv_core.IplImage frame) Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).
void	reset() Reset to initialization state.
void	updateThresh() Recalculate hsvMin and hsvMax from calH and hTol.

Methods inherited from class ohmm.CvUndistort

camIndexOptional, cmdHelpExt, cmdHelpExtParams, initExt, readMat, release

Methods inherited from class ohmm.CvBase

cmdHelp, dumpCaptureProperties, finalize, fmt, getCaptureProperty, getDefAppname, getDefInput, guiHelp, handleKey, handleMouse, init, init, init, init, mainLoop, makeGrabber, makeServer, mouseEventToString, msg, nowMS, save, setCaptureProperty, updateServerImage, v4l2DisableAuto, v4l2EnableAuto, v4l2GetExposure, v4l2SetExposure, waitForKeypresss, warn

Methods inherited from class java.lang.Object

clone, equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

APPNAME

public static final java.lang.String APPNAME

Application name.

See Also:

Constant Field Values

DEF_CAM

public static final int DEF_CAM

Default camera index.

See Also:

Constant Field Values

DEF_IMG_W

public static final int DEF_IMG_W

Default image size; must be set in call to CvBase.init(int, java.lang.String[]).

See Also:

Constant Field Values

DEF_IMG_H

public static final int DEF_IMG_H

Default image size; must be set in call to CvBase.init(int, java.lang.String[]).

See Also:

Constant Field Values

DEF_DBG_W

public static final int DEF_DBG_W

Default dbgW, dbgH.

See Also:

Constant Field Values

DEF_DBG_H

public static final int DEF_DBG_H

Default dbgW, dbgH.

See Also:

Constant Field Values

DEF_DBG_FREQ

public static final int DEF_DBG_FREQ

Default dbgFreq.

See Also:

Constant Field Values

DBG_R

public static final int DBG_R

Debug + radius in pixels.

See Also:

Constant Field Values

RESET

public static final int RESET

Visual servo state.

See Also:

Constant Field Values

INIT

public static final int INIT

Visual servo state.

See Also:

Constant Field Values

CAL

public static final int CAL

Visual servo state.

See Also:

Constant Field Values

WAIT

public static final int WAIT

Visual servo state.

See Also:

Constant Field Values

ORIENT

public static final int ORIENT

Visual servo state.

See Also:

Constant Field Values

DRIVE

public static final int DRIVE

Visual servo state.

See Also:

Constant Field Values

GRASP

public static final int GRASP

Visual servo state.

See Also:

Constant Field Values

HOMING

public static final int HOMING

Visual servo state.

See Also:

Constant Field Values

DONE

public static final int DONE

Visual servo state.

See Also:

Constant Field Values

DEF_CAL_RAD

public static final int DEF_CAL_RAD

Default calRad, calMin.

See Also:

Constant Field Values

DEF_CAL_MIN

public static final int DEF_CAL_MIN

Default calRad, calMin.

See Also:

Constant Field Values

DEF_MAX_DIFF_TRACK

public static final int DEF_MAX_DIFF_TRACK

Default maxDiffTrack.

See Also:

Constant Field Values

DEF_H_TOL

public static final int DEF_H_TOL

Default visual servo default threshold parameters.

See Also:

Constant Field Values

DEF_S_MIN

public static final int DEF_S_MIN

Default visual servo default threshold parameters.

See Also:

Constant Field Values

DEF_S_MAX

public static final int DEF_S_MAX

Default visual servo default threshold parameters.

See Also:

Constant Field Values

DEF_V_MIN

public static final int DEF_V_MIN

Default visual servo default threshold parameters.

See Also:

Constant Field Values

DEF_V_MAX

public static final int DEF_V_MAX

Default visual servo default threshold parameters.

See Also:

Constant Field Values

DEF_OPEN_IT

public static final int DEF_OPEN_IT

Default visual servo morphological open iterations.

See Also:

Constant Field Values

DEF_ORIENT_DONE_THRESH

public static final int DEF_ORIENT_DONE_THRESH

Default servo state pixel thresholds.

See Also:

Constant Field Values

DEF_ORIENT_SLOW_THRESH

public static final int DEF_ORIENT_SLOW_THRESH

Default servo state pixel thresholds.

See Also:

Constant Field Values

DEF_DRIVE_DONE_THRESH

public static final int DEF_DRIVE_DONE_THRESH

Default servo state pixel thresholds.

See Also:

Constant Field Values

DEF_DRIVE_SLOW_THRESH

public static final int DEF_DRIVE_SLOW_THRESH

Default servo state pixel thresholds.

See Also:

Constant Field Values

DEF_ORIENT_SLOW

public static final float DEF_ORIENT_SLOW

Default turn in place speeds in rad/sec.

See Also:

Constant Field Values

DEF_ORIENT_FAST

public static final float DEF_ORIENT_FAST

Default turn in place speeds in rad/sec.

See Also:

Constant Field Values

DEF_DRIVE_SLOW

public static final float DEF_DRIVE_SLOW

Default straight drive speeds in mm/sec.

See Also:

Constant Field Values

DEF_DRIVE_FAST

public static final float DEF_DRIVE_FAST

Default straight drive speeds in mm/sec.

See Also:

Constant Field Values

DEF_MIN_SERVO_TIME

public static final double DEF_MIN_SERVO_TIME

Default minServoTime.

See Also:

Constant Field Values

DEF_EXTEND_X

public static final float DEF_EXTEND_X

Default EE location in robot frame mm at retract/reach pose.

See Also:

Constant Field Values

DEF_RETRACT_X

public static final float DEF_RETRACT_X

Default EE location in robot frame mm at retract/reach pose.

See Also:

Constant Field Values

DEF_RETRACT_Z

public static final float DEF_RETRACT_Z

Default EE location in robot frame mm at retract/reach pose.

See Also:

Constant Field Values

DEF_EXTEND_Z

public static final float DEF_EXTEND_Z

Default EE location in robot frame mm at retract/reach pose.

See Also:

Constant Field Values

DEF_T0_UP

public static final float DEF_T0_UP

Default joints in rad in the visual servo "up" pose.

DEF_T1_UP

public static final float DEF_T1_UP

Default joints in rad in the visual servo "up" pose.

DEF_T2_UP

public static final float DEF_T2_UP

Default joints in rad in the visual servo "up" pose.

RXTX_PORT

public static final java.lang.String RXTX_PORT

RXTX port filename for test driver only.

See Also:

Constant Field Values

DEF_BALL_HEIGHT

public static final int DEF_BALL_HEIGHT

Default ballHeight.

See Also:

Constant Field Values

DEF_BALL_DIAMETER

public static final int DEF_BALL_DIAMETER

Default ballDiameter.

See Also:

Constant Field Values

DEF_TRACK_D_FUDGE

public static final int DEF_TRACK_D_FUDGE

Default trackDFudge.

See Also:

Constant Field Values

calRad

public volatile int calRad

Calibration gather box radius and min pixels.

calMin

public volatile int calMin

Calibration gather box radius and min pixels.

maxDiffTrack

public volatile int maxDiffTrack

Maximum change in track point in pixels.

orientDoneThresh

public volatile int orientDoneThresh

Servo state pixel thresholds.

orientSlowThresh

public volatile int orientSlowThresh

Servo state pixel thresholds.

driveDoneThresh

public volatile int driveDoneThresh

Servo state pixel thresholds.

driveSlowThresh

public volatile int driveSlowThresh

Servo state pixel thresholds.

orientSlow

public volatile float orientSlow

Turn in place speeds in rad/sec.

orientFast

public volatile float orientFast

Turn in place speeds in rad/sec.

driveSlow

public volatile float driveSlow

Straight drive speeds in mm/sec.

driveFast

public volatile float driveFast

Straight drive speeds in mm/sec.

minServoTime

public volatile double minServoTime

Minimum time between servo updates in seconds.

retractX

public volatile float retractX

EE location in robot frame mm at retract pose.

retractZ

public volatile float retractZ

EE location in robot frame mm at retract pose.

extendX

public volatile float extendX

EE location in robot frame mm at reach pose.

extendZ

public volatile float extendZ

EE location in robot frame mm at reach pose.

t0Up

public volatile float t0Up

Arm joints in rad in the visual servo "up" pose.

t1Up

public volatile float t1Up

Arm joints in rad in the visual servo "up" pose.

t2Up

public volatile float t2Up

Arm joints in rad in the visual servo "up" pose.

grasp

public final Grasp grasp

The Grasp object.

ohmm

public final OHMMDrive ohmm

The OHMMDrive object.

state

public volatile int state

State machine state, see class header doc.

runPending

public volatile boolean runPending

Whether the ORIENT state is pending.

enableHoming

public volatile boolean enableHoming

Whether to enable the HOMING state.

calX

public volatile int calX

Calibration pixel.

calY

public volatile int calY

Calibration pixel.

calR

public volatile int calR

Calibration pixel.

calG

public volatile int calG

Calibration pixel.

calB

public volatile int calB

Calibration pixel.

calH

public volatile int calH

Calibration pixel.

calS

public volatile int calS

Calibration pixel.

calV

public volatile int calV

Calibration pixel.

trackX

public volatile int trackX

Tracked pixel or (-1, -1) for no tracking.

trackY

public volatile int trackY

Tracked pixel or (-1, -1) for no tracking.

trackD

public volatile int trackD

Tracked blob diameter or -1 if no tracking, see estimateBall().

ballHeight

public volatile int ballHeight

Ball Z coord in world frame mm, see estimateBall().

ballDiameter

public volatile int ballDiameter

Ball diameter in world frame mm, see estimateBall().

trackDFudge

public volatile int trackDFudge

Fudge factor for trackD.

mouseX

public volatile int mouseX

Mouse location in image or (-1, -1) if none.

mouseY

public volatile int mouseY

Mouse location in image or (-1, -1) if none.

hTol

public volatile int hTol

Threshold parameters.

hMin

public volatile int hMin

Threshold parameters.

hMax

public volatile int hMax

Threshold parameters.

hAltMin

public volatile int hAltMin

Threshold parameters.

hAltMax

public volatile int hAltMax

Threshold parameters.

sMin

public volatile int sMin

Threshold parameters.

sMax

public volatile int sMax

Threshold parameters.

vMin

public volatile int vMin

Threshold parameters.

vMax

public volatile int vMax

Threshold parameters.

openIt

public volatile int openIt

Morphological open iterations.

hsvMin

public volatile com.googlecode.javacv.cpp.opencv_core.CvScalar hsvMin

HSV threshold bounds.

hsvMax

public volatile com.googlecode.javacv.cpp.opencv_core.CvScalar hsvMax

HSV threshold bounds.

hsvAltMin

public volatile com.googlecode.javacv.cpp.opencv_core.CvScalar hsvAltMin

HSV threshold bounds.

hsvAltMax

public volatile com.googlecode.javacv.cpp.opencv_core.CvScalar hsvAltMax

HSV threshold bounds.

dbgW

public volatile int dbgW

Debug image dims.

dbgH

public volatile int dbgH

Debug image dims.

dbgFreq

public volatile int dbgFreq

Debug image frequency relative to process(com.googlecode.javacv.cpp.opencv_core.IplImage) frequency.

storage

protected volatile com.googlecode.javacv.cpp.opencv_core.CvMemStorage storage

For storing contours.

contour

protected volatile com.googlecode.javacv.cpp.opencv_core.CvSeq contour

First contour in sequence.

hsvImg

protected volatile com.googlecode.javacv.cpp.opencv_core.IplImage hsvImg

Work images.

dbgImg

protected volatile com.googlecode.javacv.cpp.opencv_core.IplImage dbgImg

Work images.

dbgAltImg

protected volatile com.googlecode.javacv.cpp.opencv_core.IplImage dbgAltImg

Work images.

binImg

protected volatile com.googlecode.javacv.cpp.opencv_core.IplImage binImg

Work images.

binAltImg

protected volatile com.googlecode.javacv.cpp.opencv_core.IplImage binAltImg

Work images.

dbgFrameCountdown

protected volatile int dbgFrameCountdown

Countdown to a debug frame.

armThread

protected volatile java.lang.Thread armThread

Thread for running arm motions.

lastServoTime

protected volatile double lastServoTime

Time of last servo update.

initXYT

protected volatile float[] initXYT

Initial robot pose.

imgW

protected volatile int imgW

Image dims, detected on first call to process(com.googlecode.javacv.cpp.opencv_core.IplImage).

imgH

protected volatile int imgH

Image dims, detected on first call to process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Constructor Detail

CvVisualServo

public CvVisualServo(Grasp grasp)

Inits frame processing.

Parameters:

grasp - the Grasp object to control the robot

You must call one of the init() APIs and then CvBase.mainLoop() to complete setup and then start processing frames.

Sets defaults:

• CvBase.maxFPS = 10
• CvBase.serverFPS = 2

Change these (and other config fields) before calling init() for customization.

CvVisualServo

public CvVisualServo(Grasp grasp,
                     int calH,
                     int calX,
                     int calY)

Convenience constructor to run non-interactive.

Parameters:

grasp - the Grasp object to control the robot

calH - the calibration hue

calX - the calibration X pixel

calY - the calibration Y pixel

Inits calH, calX and calY then calls updateThresh(), thus skipping interactive calibration. Then sets initial state to WAIT with runPending = true, which has the effect of stopping any ongoing drive motion (without resetting odometry), opening the gripper and moving the arm to the lowered retract pose, and then continuing to state ORIENT. Sets enableHoming = false so that CvBase.mainLoop() will exit after grasping.

See CvVisualServo(Grasp) for further info.

Method Detail

reset

public void reset()

Reset to initialization state.

abort

public void abort()

reset() and set state DONE

updateThresh

public void updateThresh()

Recalculate hsvMin and hsvMax from calH and hTol.

handleKeyExt

protected boolean handleKeyExt(int code)

Handle custom keypresses.

Overrides:

handleKeyExt in class CvUndistort

guiHelpExt

protected void guiHelpExt()

Show help for custom keypresses.

Overrides:

guiHelpExt in class CvUndistort

handleMouse

protected void handleMouse(int event,
                           int x,
                           int y,
                           int flags)

Handle mouse clicks.

Overrides:

handleMouse in class CvBase

Parameters:

event - one of the CV_EVENT_* constants (see opencv_highgui.java in the JavaCV sources for a list)

x - the x pixel coordinate of the mouse event

y - the y pixel coordinate of the mouse event

flags - bitmask of the CV_EVENT_FLAG_* constants (see opencv_highgui.java in the JavaCV sources for a list)

Default impl prints a message using CvBase.mouseEventToString(int, int, int, int) except for CV_EVENT_MOUSEMOVE.

Overriding this is one way that subclasses can handle mouse events.

process

protected com.googlecode.javacv.cpp.opencv_core.IplImage process(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Do all the frame processing and servoing.

See class header doc for details.

Overrides:

process in class CvUndistort

Returns:

the processed image, null will cause image display to be skipped for this frame

processReset

protected void processReset(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processCal

protected void processCal(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processWait

protected void processWait(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processUpdateTracking

protected void processUpdateTracking(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Update trackX, trackY in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Called iff state is at least WAIT.

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processOrient

protected void processOrient(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processDrive

protected void processDrive(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processGrasp

protected void processGrasp(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processHoming

protected void processHoming(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Handle the indicated state in process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

processUpdateReturnImge

protected com.googlecode.javacv.cpp.opencv_core.IplImage processUpdateReturnImge(com.googlecode.javacv.cpp.opencv_core.IplImage frame)

Create/update the return image for process(com.googlecode.javacv.cpp.opencv_core.IplImage).

Parameters:

frame - the image passed to process(com.googlecode.javacv.cpp.opencv_core.IplImage)

Returns:

the new image to display or null for none

doneProcessing

protected boolean doneProcessing()

This impl checks if state is DONE.

Overrides:

doneProcessing in class CvBase

Returns:

true iff main loop should end

Default impl returns false.

estimateBall

protected void estimateBall()

Estimate ball coordinates in robot frame (optional).

The main visual servo algorithm does not require this estimate and does not use this method, which is for demonstration only.

Demonstrates two approaches to estimate the 3D ball coordinates in robot frame mm. One is based on the assumption that the center of the ball is at ballHeight mm above the ground. The other assumes the ball diameter is ballDiameter mm and estimates the apparrent diameter in pixels.

Both methods require the camera matrix CvUndistort.intrinsics and the camera pose CvUndistort.extrinsics in robot frame, which are only available if a camera calibration file was specified on the command line.

main

public static void main(java.lang.String[] argv)

Test driver.