Panda Pick-and-Place Pipeline

Why red is detected with two hue ranges, not one赤の検出に色相範囲を2つ使う理由

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は色（色相）を明るさや鮮やかさから分離します。そのため、色の閾値処理には生のRGBではなくHSVを使います。影の中の赤いボールと光の当たった同じボールでは、RGB値は大きく違っても色相はほぼ同じだからです。 OpenCVは色相を0〜179で表します。赤はその継ぎ目にあり、純粋な赤は色相0、やや紫寄りの赤は色相179です。[0, 10]の1範囲だけでは半分を取りこぼします。そこでnodeは2つのマスクを作り、ORで結合します。

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.

2番目と3番目の値も色相と同じくらい重要です。彩度 ≥ 120は、たまたま赤寄りになった色あせた灰色を除外し、明度 ≥ 80は、色相が意味のないノイズになる黒に近いピクセルを除外します。

Open, then closeopenしてからclose

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

生のマスクは境界付近にノイズがあります。7×7の楕円カーネルを使った2つのモルフォロジー演算で整えます。

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.

順序が重要です。先にcloseすると、openで消す前に、近くにある2つのノイズが1つの偽物体に融合してしまいます。

Centroid from image moments, not from the bounding box重心は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.

400 px²の面積フィルタを通過した輪郭は、モーメントから中心を計算します（u = m10/m00、v = m01/m00）。 これは面積で重み付けしたピクセル位置の平均で、サブピクセル精度の値になります。外接矩形の中心と違い、ギザギザした境界にも強い手法です。 円形のボールでは差は小さいものの、一部が欠けた領域では大きな差になります。

Back-projection, and where the intrinsics come fromback-projectionと内部パラメータの出どころ

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

ピクセルをメートルに変換する2行は、コード上では次のとおりです。

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.

符号の反転と軸の入れ替えに注意してください。画像のuは右方向に増え、ロボットのyに対応します。画像のvは下方向に増え、マイナス符号付きでロボットのxに対応します。 ここを間違えると、システムはエラーなく動いたまま、すべてのボールの鏡像の位置へ手を伸ばします。まさに例外が一切出ない種類のバグです。

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.

検出器はfx, fy, cx, cyのハードコードされた初期値で起動しますが、/camera/camera_infoにCameraInfoメッセージが届いた時点で上書きします。 実際のドライバもこの方法でcalibrationをpublishするため、nodeは実機と同じやり方で読み込みます。YAMLから読むのはカメラの取り付け位置である外部パラメータだけで、これはシミュレータと共有しているため、両者は構造的に一致します。

Why cv_bridge was replaced by 40 linescv_bridgeを40行のコードに置き換えた理由

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.

cv_bridgeは、OpenCVの配列とsensor_msgs/Imageを相互変換するROSの標準ライブラリです。C++から約20種類のピクセルエンコーディングを扱うためのもので、特定のNumPyメジャーバージョンにリンクしたコンパイル済み拡張として配布されています。 このパッケージでは画像経路の両端がどちらも内部実装で、どちらもbgr8なので、変換はreshapeとコピーだけで済みます。

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.

難しいのは、バイト単位の行ストライドであるstepです。publisherは行をパディングすることがあるため、stepが常にwidth × 3とは限らず、0のままにするpublisherもあります。 コードは0で割らず、0を「パディングなし」として扱います。割り算をすると幅0の画像にreshapeされ、ずっと後でまったく別の原因に見えるエラーで失敗するからです。 続いて、ストライドがピクセル数の整数倍であることとバッファ長が足りていることを検証し、パディングを切り落とし、OpenCVが書き込み可能な連続配列を受け取れるようにコピーします。

The C++ reachability filterC++の到達可能性フィルタ

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

Franka Pandaはどこにでも届くわけではありません。validatorは作業領域を円筒で近似し、4つの境界値と1つの追加ルールを適用します。

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.

除外した対象は黙って捨てず、理由の文字列とともにログに残します。残った点は、姿勢を単位回転にしたPoseArrayとして再publishします。validatorが判断するのはどこかであって、どう掴むかではありません。 2秒ごとにRVizへ描く2つの円筒は同じ境界値を可視化したものなので、届かないボールはログを読む前に画面上で一目でわかります。

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.

このnodeをPythonではなくC++で書いたのには明確な理由があります。10 Hzのカメラフレームごとに実行されるため、コントローラを待たせる原因になってはならないからです。 また、実際のROS 2システムがそうであるように、複数言語が混在するワークスペースを示すのにも自然な場所です。

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.

ここには本質的な矛盾があります。MoveItはcollision objectの内部で終わる経路を計画しません。しかしgrasp poseは、定義上ボールの位置にあります。 教科書どおりの答えであるボールをグリッパーに取り付ける方法（AttachedCollisionObject）も試しましたが、panda_link7との自己干渉で失敗し、さらにグリッパーに緑の球が貼り付いて表示される不具合も出ました。

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.

解決策は、ボールを2つに分けることです。衝突判定用のボールはアプローチ直前に削除し、プランナが障害物のないゴールを得られるようにします。 同じ位置には表示用のボール（物理も衝突判定も持たないRVizのMarker）を出しておき、オペレータがアプローチ中もボールを確認できるようにし、グリッパーが到着した瞬間に削除します。 配置動作が成功したら、箱の中に新しい衝突判定用の球を追加します。画面上は見る人の期待どおりになり、プランナがこれから拾う障害物と競合することもありません。

Cartesian first, OMPL secondまずCartesian、次にOMPL

Two ways to get the gripper from A to B:

グリッパーをAからBへ動かす方法は2つあります。

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.

service /compute_cartesian_path。エンドエフェクタを1 cm刻みで直線補間し、各点で逆運動学を解きます。 ボールの上まで滑らかに移動して真下へ、真上に上げて箱へ、と予測しやすく人が読み取れる動きになります。要求した経路のうち実際に達成できた割合をfractionとして返し、0.95未満なら結果を棄却します。

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.

action /move_action。関節空間内で衝突のない経路を何かしら見つけるサンプリングベースのプランナです。経路が存在すれば必ず見つけますが、目的関数に整った経路を求める項がないため、手首が大きく危なっかしく振れる経路になることがあります。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.

コントローラはまずCartesianを試し、失敗したら同じ対象を制約付きのOMPLゴールとして表現してfallbackします。デモの動きが無秩序ではなく意図的に見えるのは、この順序のおかげです。

Retiming the trajectory afterwards計画後の軌道のretiming

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:

OMPLの経路ではmax_velocity_scaling_factorを指定できます。しかしHumbleのGetCartesianPath serviceにはこのフィールドがなく、全速の時間配分で軌道を返します。 そこで、計画後にコード側で時間を引き伸ばします。

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.

cartesian_speed_scale = 0.5なら、すべての動作に2倍の時間がかかります。加速度の項が2乗になるのは、加速度の単位が距離÷時間の2乗だからです。 速度だけをスケールすると、数値同士が矛盾した軌道をコントローラに渡すことになり、まともなコントローラはそれを拒否します。

The goal constraintsゴールの制約

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

OMPLのゴールはposeではなく、制約の集合です。コントローラは2つの制約を作ります。

• 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.

• 位置：対象を中心とする箱形の領域で、一辺は把持時4 cm、配置時6 cm。リンクpanda_link8に対して設定します。 点ではなく許容領域にしているのは、7軸アームに厳密なposeを要求すると、IKソルバの負担が大きく増えるだけで利点がないためです。
• 姿勢：グリッパーが下を向いたままになるよう、x軸とy軸の傾き許容値は0.25 radですが、z軸は3.14 rad、つまり自由です。 球は手首をどの角度に回しても同じに見えるため、その回転を制約しても、プランナが使えたはずの解を減らすだけです。

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.

すべての把持でyaw = 0°を使うため、把持から配置、次の把持まで手首の向きが変わりません。そのぶん、OMPLが探す関節空間の経路も単純になります。

Making an asynchronous action look synchronous非同期のactionを同期的に見せる

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.

ROS 2のactionは2段階のcallback駆動です。goalを送ると受理されたかを示すhandleが返り、さらに結果用のcallbackを登録します。 そのまま書くと、5ステップのpick処理は5段にネストしたcallbackになります。_send_goal()はthreading.Eventと要素1つのリストを使ってclosureの外へ結果を持ち出し、60秒のタイムアウト付きでブロックすることで、これを平坦にしています。 その結果、state machineは上から順に読めるコードになります。

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.

これが成り立つのは、nodeをMultiThreadedExecutorでspinし、action clientをReentrantCallbackGroupに入れ、さらにpickループを専用スレッドで回しているからです。 シングルスレッドのexecutorでは、ブロッキング待ちが、待っているcallbackそのものの実行を妨げてしまいます。ROS 2の典型的なデッドロックです。