Panda Pick-and-Place Pipeline

Why red is detected with two hue ranges, not one

HSV separates colour (hue) from brightness and vividness, which is why it is used for colour thresholding instead of raw RGB — a red ball in shadow and the same ball in light have very different RGB values but nearly the same hue. OpenCV stores hue as 0–179. Red sits at the seam: pure red is hue 0, and slightly-purple red is hue 179. A single window [0, 10] would miss half of it. So the node builds two masks and ORs them together:

hsv_red_lo1: [0,   120, 80]     hsv_red_hi1: [10,  255, 255]
hsv_red_lo2: [170, 120, 80]     hsv_red_hi2: [180, 255, 255]

The second and third numbers matter as much as the hue: saturation ≥ 120 excludes washed-out greys that happen to lean red, and value ≥ 80 excludes near-black pixels where hue is meaningless noise.

Open, then close

The raw mask is noisy at the edges. Two morphological operations clean it with a 7×7 elliptical kernel:

MORPH_OPEN

Erode then dilate. Removes isolated specks smaller than the kernel without shrinking the real blobs.

MORPH_CLOSE

Dilate then erode. Fills small holes inside a blob — such as the white outline stroke drawn around each rendered ball.

Order matters. Closing first would fuse two nearby specks into one fake object before opening had a chance to delete them.

Centroid from image moments, not from the bounding box

Once a contour survives the 400 px² area filter, the centre is computed from moments: u = m10/m00, v = m01/m00. That is the area-weighted average pixel position — a sub-pixel value, and it is robust to a ragged edge in a way that the centre of a bounding rectangle is not. For a circular ball the difference is small; for a partly-clipped blob it is not.

Back-projection, and where the intrinsics come from

The two lines that turn pixels into metres are, in the code:

wy = cam_y + (u - cx) * cam_h / fx
wx = cam_x - (v - cy) * cam_h / fy
wz = 0.0        # DETECT_Z — the table plane assumption

Note the sign flip and the axis swap. Image u grows to the right and maps to the robot's y; image v grows downward and maps to the robot's x with a minus sign. Getting this wrong produces a system that runs cleanly and reaches for the mirror image of every ball, which is exactly the class of bug that never raises an exception.

The detector starts with hard-coded defaults for fx, fy, cx, cy but overwrites them the moment a CameraInfo message arrives on /camera/camera_info. That is how a real driver publishes calibration, so the node reads it the way it would read a real one. Only the extrinsics — where the camera is mounted — come from the YAML, and they are shared with the simulator so both ends agree by construction.

Why cv_bridge was replaced by 40 lines

cv_bridge is the standard ROS library for converting between OpenCV arrays and sensor_msgs/Image. It exists to handle roughly twenty pixel encodings from C++, and it ships as a compiled extension linked against a specific NumPy major version. In this package both ends of the image path are internal and both are bgr8, so the conversion is a reshape and a copy.

The tricky part is step, the row stride in bytes. A publisher may pad rows, so step is not always width × 3, and some publishers leave it at 0. The code treats 0 as unpadded rather than dividing by it — dividing would reshape to a zero-width image and fail much later with an error about something else entirely. It then validates that the stride is a whole number of pixels and that the buffer is long enough, slices off the padding, and copies so OpenCV receives a writable contiguous array.

The C++ reachability filter

A Franka Panda cannot reach everywhere. The validator approximates its workspace as a cylinder and applies four bounds plus one extra rule:

Rejections are logged with a reason string rather than silently dropped, and the surviving points are republished as a PoseArray with identity orientation — the validator has an opinion about where, not about how. The two RViz cylinders it draws every two seconds are the same bounds made visible, so an out-of-reach ball is obvious on screen before you read a log line.

This node exists in C++ rather than Python for a stated reason: it runs on every camera frame at 10 Hz and must never become the reason the controller waits. It is also the natural place to demonstrate a multi-language workspace, which is what real ROS 2 systems are.

The ghost-ball trick

There is a genuine conflict here. MoveIt refuses to plan a path that ends inside a collision object — and the grasp pose is, by definition, at the ball. Attaching the ball to the gripper (AttachedCollisionObject, the textbook answer) was tried and caused self-collision failures against panda_link7 plus a green sphere artefact stuck to the gripper.

The solution splits the ball into two things. The collision ball is removed just before the approach so the planner has a clear goal. A visual ball — an RViz Marker, which has no physics and no collision meaning — is shown in the same place so the operator still sees it during the approach, then deleted the instant the gripper arrives. After the place motion succeeds, a fresh collision sphere is added inside the box. The result on screen is what a viewer expects, and the planner never fights an obstacle that is about to be picked up.

Cartesian first, OMPL second

Two ways to get the gripper from A to B:

Cartesian path

Service /compute_cartesian_path. Interpolates the end-effector along a straight line in 1 cm steps and solves inverse kinematics at each one. Predictable, human-legible motion — glide over the ball, straight down; straight up, across to the box. Returns a fraction: how much of the requested path it actually managed. Below 0.95 the result is rejected.

OMPL RRTConnect

Action /move_action. A sampling-based planner that finds some collision-free path through joint space. It will always find one if it exists, but the path can include large, alarming wrist excursions because nothing in the objective asks for a tidy route. Used only as a fallback.

The controller tries Cartesian, and on failure falls back with the same target expressed as a constrained OMPL goal. That ordering is why the demo looks deliberate rather than chaotic.

Retiming the trajectory afterwards

The OMPL path lets you request max_velocity_scaling_factor. Humble's GetCartesianPath service has no such field — it returns a trajectory timed at full speed. So the code stretches the timing after planning:

t_new       = t_old / scale
velocities  ×= scale
accelerations ×= scale²

With cartesian_speed_scale = 0.5 everything takes twice as long. The acceleration term is squared because acceleration has units of distance per time squared — scaling only the velocities would hand the controller a trajectory whose numbers contradict each other, and a well-behaved controller will reject it.

The goal constraints

An OMPL goal is not a pose, it is a set of constraints. The controller builds two:

• Position — a box-shaped region 4 cm on a side for a grasp, 6 cm for a place, centred on the target, attached to link panda_link8. A tolerance region rather than a point, because demanding an exact pose from a 7-axis arm makes the IK solver work far harder for no benefit.
• Orientation — tilt tolerance 0.25 rad on the x and y axes so the gripper stays pointing down, but 3.14 rad on z, which is to say free. A sphere looks identical from every angle of wrist rotation, so constraining that rotation only removes solutions the planner could have used.

All grasps use yaw = 0° so the wrist keeps the same orientation from grasp to place to the next grasp, which gives OMPL simpler joint-space paths to find.

Making an asynchronous action look synchronous

ROS 2 actions are two-stage and callback-driven: send a goal, get a handle telling you whether it was accepted, then register another callback for the result. Written literally, a five-step pick sequence becomes five levels of nested callbacks. _send_goal() collapses that using a threading.Event and a one-element list to smuggle the result out of the closure, then blocks with a 60-second timeout. The state machine reads as straight-line code.

This only works because the node spins on a MultiThreadedExecutor with the action clients in a ReentrantCallbackGroup, and because the pick loop runs on its own thread. On a single-threaded executor the blocking wait would prevent the very callback it is waiting for from ever running — a classic ROS 2 deadlock.