简介基于ROS2与FrankaPanda机械臂的颜色分拣机器人完整工程面向机器人课程设计、毕业设计及AI视觉分拣入门者展示视觉识别、运动规划与协作机械臂控制的整合方法。压缩包共112个文件大小5.78MB涵盖40个py控制/视觉脚本、14个dae与10个obj模型、8个yaml与6个xacro机器人描述配置、2个dockerfile容器部署文件以及rviz、world、sdf等仿真相关资源模块化组织便于逐项学习。项目以panda_vision完成颜色识别经pymoveit2调用MoveIt!规划配合panda_moveit、panda_controller、panda_bringup等包实现从图像输入到抓取分拣的完整流程并保证开发环境一致性。已有63人学习适合初学者按README快速复现也可作为二次开发的工程参考。1. 基于ROS2的FrankaPanda颜色分拣机器人是仿真工程不是现成的分拣产线作为毕业设计或者课程设计的起点这份资源要解决的问题很直接用 ROS2 Humble 在 Gazebo 里搭一台 FrankaPanda 七自由度机械臂挂一个仿真相机识别桌面上红、绿、蓝三种颜色的物料块再由 MoveIt2 规划抓取轨迹把它们放到指定的分拣区域。刚拿到这个包时我和多数人预期一样以为编译完就能看机械臂自己干活实际打开才发现这是一个需要自己动手配 TF、调 HSV 阈值、改 MoveIt2 参数的完整仿真工程。这个工程骨架的意义不在于“开箱即动”而在于它能让你从头到尾走完视觉识别、坐标变换、运动规划、抓取执行这条链路。适合已经装好 ROS2 Humble、愿意碰 CMakeLists 和 launch 文件的同学。2. FrankaPanda的颜色分拣链路从相机像素到MoveIt2的规划目标很多人第一反应是“颜色分拣嘛核心在视觉”于是把精力全砸在训练 YOLO、调深度学习模型上方向就偏了。Gazebo 仿真环境里光照稳定、物体单一颜色分拣的 90% 工作量其实在坐标变换和运动规划参数上。要理清这份资源先把整条数据链看明白相机采集图像 → 视觉节点输出目标像素 → 像素坐标换到机器人基座标系 → MoveIt2 拿到目标位姿做规划 → 机械臂执行抓取与放置。2.1 Gazebo里的相机模型内参、图像话题与HSV颜色空间Gazebo 里给 Panda 挂的仿真相机常见配置是分辨率 640×480、编码 bgr8话题名一般是/camera/image_raw。相机装在桌面上方某个固定支架上朝向桌面这样机械臂运动时相机不会跟着抖。这个相机模型在 SDF 文件里定义了内参包括焦距 fx、fy 和光心 cx、cy。要知道 fx、fy 是多少直接看世界文件的camera字段或者启动后用ros2 topic echo /camera/camera_info读一次。颜色识别用 RGB 还是 HSV仿真里光线恒定RGB 确实“能跑”但真实场景里 RGB 对亮度太敏感同一张图在不同曝光下阈值完全没法复用。HSV 把色相、饱和度、明度分开颜色判断只看 H 通道。OpenCV 里 H 范围是 0~179对应 HSV 色环的 0~360°。常见阈值区间我列在下面颜色H 范围S 下限V 下限注意点红0~10 与 160~179 两段合并8060红色跨过 0 度边界必须做两段合并绿35~858060仿真里绿色最稳定区间可以放宽蓝100~1308060蓝和绿在低饱和度时容易混淆用cv2.inRange生成掩膜后再做连通域分析取最大轮廓的质心得到像素坐标 (u, v)。这一步有个小陷阱红色掩膜必须把 0~10 和 160~179 两个区间合并成一个布尔掩膜只写一个区间会导致红色物体在色环边界附近被截断。提示仿真环境下光照恒定S、V 下限可以放宽到 30~40但不要把上限放宽太多否则桌面的反光区域会被误判成物体。2.2 从像素坐标到抓取位姿TF树的组装逻辑拿到了像素坐标 (u, v)下一步要把它变成机器人能用的三维目标点。常见做法有两种一是从深度图取物体中心点的 Z 值二是直接假设物体放在固定高度比如桌面上方 0.03 m上。这份资源里物体形状规则、桌面水平固定高度方案是主流选择因为它省去了对齐深度图的麻烦但代价是桌面一倾斜或者物体被垫高就翻车。像素坐标换到相机坐标系下的三维点按针孔相机模型走Z fixed_height # 物体在相机坐标系下的深度比如 0.52 m X (u - cx) * Z / fx Y (v - cy) * Z / fy这里的 fx、fy、cx、cy 从camera_info话题读。Z 取多少取决于相机安装高度和桌面的相对位置算一次之后就固定了。有了相机坐标系下的 (X, Y, Z)再用 TF 把它变换到机器人基座标系panda_link0。这条 TF 链一般是camera_link → camera_color_optical_frame → panda_link0相机光轴和机器人基座之间的相对位置用静态 TF 发布即可因为相机是固定不动的。常见做法是直接写进 launch 文件ros2 run tf2_ros static_transform_publisher \ 0.20 0.0 0.50 0 0 0 \ camera_link panda_link0参数说明前三个数是平移量单位米含义是相机坐标系原点在机器人基座坐标系下的位置后三个数是绕 x、y、z 轴的旋转角单位弧度全 0 表示相机朝向与基座坐标系一致。如果你的相机是斜着装或者吊装的这三个角度必须从 SDF 文件里抄出来不要自己估否则后续抓取位姿会偏得离谱。2.3 MoveIt2的角色规划场景与动作执行MoveIt2 不负责控制电机它解决的是“怎么从当前关节角走到目标位姿”的规划问题。在这份资源里Panda 的 MoveIt2 配置一般包含两个关键对象规划组panda_manipulator末端执行器panda_hand。做颜色分拣时MoveIt2 要完成四段动作阶段动作关键参数pre-grasp末端移到目标上方 5~10 cm保持末端朝下Z 轴方向与桌面垂直grasp下降到物体高度并闭合夹爪下降距离由物体高度决定别直接怼到桌面上lift夹住物体垂直抬起抬升高度根据避障需要一般 0.1 m 以上place移动到放置区上方并松开需要第二个目标位姿与抓取位姿分开计算这里最容易忽略的是 Planning Scene。Gazebo 世界里的物体不会自动进入 MoveIt2 的碰撞检测环境你必须把物料块的尺寸、位置通过collision object手动加进规划场景不然机械臂会“无视”物料块直接穿过去。规划参数方面planning time 常用 2~5 秒规划器优先选 RRTConnect它对七自由度机械臂的求解成功率明显高于 PRM。3. 核心代码怎么串检测、位姿发布与抓取执行闭环这一章把资源里的代码通读一遍。项目的功能包划分很清楚三个功能包各管一段职责视觉、控制、启动互不干扰。3.1 工程目录src下的三个功能包资源里的工作空间结构大致如下ros2_franka_color_sort/ ├── src │ ├── franka_color_detect/ # 颜色识别输出目标位姿 │ ├── franka_pick_place/ # 规划执行接收位姿并抓取 │ └── franka_sort_bringup/ # 世界模型、TF、launch文件 └── install/ # 编译产物勿手动改动franka_color_detect只做一件事订阅图像发布PoseStamped到/color_sort/target_pose。franka_pick_place订阅这个位姿调用 MoveIt2 执行抓取。franka_sort_bringup负责把 Gazebo 世界、静态 TF、rviz2 配置串起来。这个解耦思路值得抄进自己的工程视觉节点不依赖 MoveIt2控制节点不关心颜色怎么识别两个包可以单独测试。3.2 颜色检测节点OpenCV的HSV判断与目标话题发布视觉节点用 Python 写核心是cv2.inRange加连通域分析再转成PoseStamped发布出去。代码骨架如下import cv2 import numpy as np import rclpy from rclpy.node import Node from sensor_msgs.msg import Image from geometry_msgs.msg import PoseStamped from cv_bridge import CvBridge class ColorDetect(Node): def __init__(self): super().__init__(color_detect) self.sub self.create_subscription( Image, /camera/image_raw, self.image_cb, 10) self.pub self.create_publisher( PoseStamped, /color_sort/target_pose, 10) self.bridge CvBridge() self.cx 320.0 self.cy 240.0 self.fx 554.0 self.fy 554.0 self.fixed_z 0.52 # 物体到相机的深度按实际标定 def image_cb(self, msg): frame self.bridge.imgmsg_to_cv2(msg, bgr8) hsv cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask cv2.inRange(hsv, (35, 40, 40), (85, 255, 255)) # 绿色区间 contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if not contours: return c max(contours, keycv2.contourArea) M cv2.moments(c) if M[m00] 1e-6: return u M[m10] / M[m00] v M[m01] / M[m00] pose PoseStamped() pose.header.frame_id camera_link pose.pose.position.x (u - self.cx) * self.fixed_z / self.fx pose.pose.position.y (v - self.cy) * self.fixed_z / self.fy pose.pose.position.z self.fixed_z pose.pose.orientation.w 1.0 self.pub.publish(pose) rclpy.init() node ColorDetect() rclpy.spin(node)逻辑说明先从image_raw解码成 BGR 帧转 HSV 后按绿色区间(35, 40, 40) ~ (85, 255, 255)生成掩膜findContours找到所有绿色连通域取面积最大的那个cv2.moments算质心得到像素坐标。最后按X (u - cx) * Z / fx的公式换到相机坐标系发布PoseStamped。参数说明这个节点里最值得调的是fixed_z。它的含义是物体表面到相机光心的深度不是桌面到相机的高度。如果视觉节点发布的抓取点总是偏高或偏低先别动 HSV 阈值优先重新标定这个值。fx/fy从camera_info里读不要用默认值硬算每个模型的相机内参都有差异。3.3 抓取执行MoveGroupInterface把目标位姿变成机械臂动作控制节点用 C 写 MoveIt2 接口更常见C 的MoveGroupInterface比 Python 封装更完整。核心流程是订阅目标位姿设置 pre-grasp 偏移规划并执行#include moveit/move_group_interface/move_group_interface.h #include geometry_msgs/msg/pose_stamped.hpp moveit::planning_interface::MoveGroupInterface group(panda_manipulator); group.setPlannerId(RRTConnect); group.setPlanningTime(5.0); geometry_msgs::msg::PoseStamped target; target.header.frame_id panda_link0; target.pose.position.x msg.pose.position.x; // 来自视觉节点 target.pose.position.y msg.pose.position.y; target.pose.position.z msg.pose.position.z 0.06; // pre-grasp 抬高 target.pose.orientation msg.pose.orientation; group.setPoseTarget(target); moveit::planning_interface::MoveGroupInterface::Plan plan; bool ok group.plan(plan) moveit::core::MoveItErrorCode::SUCCESS; if (ok) { group.execute(); }这段代码的核心是把视觉节点发来的位姿先加一个0.06 m的偏移让机械臂先运动到物体上方再执行下一步的下降抓取。直接拿视觉位姿去规划末端会直接撞到桌面上。setPoseTarget要求的是末端在panda_link0坐标系下的位姿所以视觉节点发布时frame_id必须是panda_link0或先做了transformPose变换不是简单把camera_link的坐标直接塞进去。提示C 节点的 CMakeLists 里要 linkmoveit_planning_interface、moveit_ros_planning_interface这两个库编译报 undefined reference 基本都是少了链接。4. 避坑与常见问题排查Gazebo时序、TF树和碰撞检测的几个大坑从拿到这份资源到机械臂真正把彩色物料块放进分拣区中间翻车的概率很大。以下几条是我实际跑通后整理出的高频坑每条都按“现象 → 原因 → 解决”写。4.1 相机话题能看画但detect节点收不到启动顺序和DDS策略现象Gazebo 里已经能正常显示图像话题但color_detect节点一直打印“等待图像”。原因Gazebo 启动时图像话题发布慢或者 DDS 的共享内存配置导致大分辨率图像丢消息。sensor_msgs/Image在 640×480 分辨率下每帧约 900 KB跨进程频繁订阅时容易触发 RMW 的丢包策略。解决先启动视觉节点再启动 Gazebo 仿真保证订阅端先就绪或者把图像分辨率降到 320×240 降低带宽压力。如果用了 Docker在启动容器时加上--shm-size1g因为共享内存默认 64 MB 根本不够图像话题跑。4.2 TF广播超时MoveIt2一规划就报Lookup would time out现象Rviz2 里模型显示正常但一旦点击 Plan终端就刷Lookup would time out规划的轨迹出不来。原因Gazebo 仿真的实时因子不稳TF 树有抖动MoveIt2 默认的 TF 超时时间只有 100 ms稍微卡一下就直接失败。解决在控制节点里把 TF 等待时间加到 5 秒用waitForTransform接口先确认变换可用再规划。同时检查 launch 文件里有没有设置use_sim_time:true如果没有整个 TF 树的时间戳和仿真时钟对不上超时是必然的。这个问题是全过程里最容易忽略的一个——因为 Rviz2 里看起来一切正常。4.3 夹爪看起来碰到球了但抓不起来PlanningScene里没有物料块现象Rviz2 里机械臂轨迹看起来和物料块重叠但没碰撞执行后物理仿真里物料块被弹飞或者夹爪闭合后什么都没抓住。原因MoveIt2 的碰撞检测只认 Planning Scene 里的物体Gazebo 世界里的物料块不会自动进入 MoveIt2。机械臂规划时认为路径上没有障碍自然直接穿模。解决把物料块的尺寸、位置通过collision object接口加进 Planning Scene。常见做法是在控制节点里订阅物料块的位置然后构造moveit_msgs::msg::CollisionObject用applyCollisionObject加上。加了之后再看轨迹规划器会自动避开物料块表面抓取动作也会明显“迟疑”一下再贴近这是正常的。4.4 规划速度慢得像PPT规划器与时间参数没调现象一次 Plan 要 5~10 秒有时候还失败机械臂的动作一顿一顿的。原因默认的规划器是PRM对七自由度机械臂求解效率低加上planning_time默认值偏大碰撞检测在笛卡尔空间采样过密。解决规划器换成RRTConnectplanning_time从默认的 5 秒降到 1~2 秒goal_tolerance放宽到0.02 m / 0.02 rad。RRTConnect 的优势是双树同时扩张抓取这类“从开位到闭位”的直线路径求解成功率明显更高。4.5 夹爪报错或一直不出爪末端执行器名字对不上现象group.plan()成功了execute()报panda_hand的 joint 名称找不到夹爪不动。原因MoveIt2 配置文件里的末端执行器实际叫panda_hand还是hand取决于ros2_controllers和 SRDF 的定义。资源包从不同仓库合并过来时名字经常对不上。解决用ros2 param list查看运行中的 move_group 参数找到robot_description_semantic里的end_effector标签照抄实际名称。改完planning_group和end_effector两个参数后重新编译即可。这个问题属于配置文件层面的错配排查优先级最高。5. 不依赖真机也能验证分拣逻辑ros2 bag回放与慢速轨迹确认仿真环境最大的优势是可以离线验证。每改一次 HSV 阈值或者规划参数都要重启 Gazebo光启动就浪费几分钟这个成本在调参阶段完全没必要。我自己的习惯是先把整个分拣过程用 ros2 bag 录下来然后离线回放验证视觉与规划逻辑。先建一个目录启动仿真、视觉节点和控制节点然后录制与分拣相关的所有话题ros2 bag record -o color_sort_01 \ /camera/image_raw \ /camera/camera_info \ /color_sort/target_pose \ /panda/joint_states \ /panda_arm_controller/joint_trajectory录制的四组话题对应四个验证维度图像源、视觉输出、关节状态、控制指令。回放时用--rate参数放慢速度比如半速回放ros2 bag play color_sort_01 --rate 0.5回放时打开 Rviz2把MotionPlanning面板的Planned Path勾上选中panda_manipulator逐帧查看末端轨迹有没有穿模、抓取点有没有落在物料块正中心。这一步能验证的细节比实时仿真多很多因为可以暂停、回退、反复看同一帧。验证标准可以列成一组简单的检查项验证项检查方法预期结果颜色识别回放/camera/image_raw观察 detect 输出的位姿目标位姿稳定在物体中心不跳变轨迹规划Rviz2 查看规划路径路径不穿物料块无急转弯抓取时序慢速回放看低速阶段先 pre-grasp、再下降、再闭合夹爪放置精度测量放置区中心与目标位姿距离误差在 2 cm 以内慢速回放时重点检查抓取时序。一个典型的错误是机械臂还在下降过程中夹爪就闭合了说明 grasp 动作和下降动作没有串成顺序执行。实时仿真里跑得太快根本看不清一慢速回放就暴露了。另外可以把group.setMaxVelocityScalingFactor(0.1)加进控制节点临时把速度降到 10% 再跑一次规划。这不算作弊低速运行能让夹爪与物料块的接触仿真更稳定也能让你看清整个动作的准备和收尾。从那以后我拿到任何 Gazebo 仿真工程第一件事都是先 ros2 bag record 录一遍再动手改参数而不是反复重启世界等它自己跑到目标点。这个习惯帮我省下了大量无效调试时间希望帮到你。本文还有配套的精品资源点击获取