基于 YOLOv8 的康乃馨自动采收机：第四章控制系统整理与源码摘录

src/
├─ carnation_description/
│  ├─ config/
│  │  └─ display.rviz
│  ├─ launch/
│  │  └─ display.launch.py
│  ├─ meshes/
│  │  ├─ base_link.stl
│  │  ├─ upper_lift_link.stl
│  │  ├─ lower_lift_link.stl
│  │  ├─ upper_arm_link.stl
│  │  └─ lower_arm_link.stl
│  └─ urdf/
│     └─ carnation_robot_v0.urdf
├─ carnation_planning/
│  ├─ carnation_planning/
│  │  └─ simple_target_follower.py
│  ├─ config/
│  │  └─ simple_target_follower.yaml
│  └─ launch/
│     └─ simple_alignment.launch.py
├─ carnation_interfaces/
├─ carnation_localization/
├─ carnation_perception/
├─ carnation_simulation/
└─ carnation_task_manager/

<?xml version="1.0"?>

<robot name="carnation_robot">
  <link name="world"/>
  <link name="world_aligned_link"/>

  <joint name="world_align_joint" type="fixed">
    <parent link="world"/>
    <child link="world_aligned_link"/>
    <origin xyz="0 0 0.20" rpy="1.5708 0 0"/>
  </joint>

  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://carnation_description/meshes/base_link.stl"
              scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>

  <joint name="base_move_joint" type="prismatic">
    <parent link="world_aligned_link"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0.0" upper="5.0" effort="500.0" velocity="0.5"/>
  </joint>

  <joint name="upper_lift_joint" type="prismatic">
    <parent link="base_link"/>
    <child link="upper_lift_link"/>
    <origin xyz="0 -0.12 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="0.0" upper="0.20" effort="200.0" velocity="0.05"/>
  </joint>

  <joint name="upper_extend_joint" type="prismatic">
    <parent link="upper_lift_link"/>
    <child link="upper_arm_link"/>
    <origin xyz="0 0 0.12" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="0.0" upper="-0.37" effort="150.0" velocity="0.05"/>
  </joint>

  <joint name="lower_lift_joint" type="prismatic">
    <parent link="base_link"/>
    <child link="lower_lift_link"/>
    <origin xyz="0 -0.05 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="0.0" upper="0.20" effort="200.0" velocity="0.05"/>
  </joint>

  <joint name="lower_extend_joint" type="prismatic">
    <parent link="lower_lift_link"/>
    <child link="lower_arm_link"/>
    <origin xyz="0 0 0.08" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="0.0" upper="-0.37" effort="150.0" velocity="0.05"/>
  </joint>
</robot>

这个配置文件把各关节的坐标映射、零点、缩放系数、限位和速度集中到参数层， 使 simple_target_follower 可以通过 YAML 调整，而不必反复改 Python 代码。

simple_target_follower:
  ros__parameters:
    # 节点循环频率，决定 joint_states 的刷新速度
    publish_rate_hz: 20.0

    # 要求输入目标位姿位于 world 坐标系中
    expected_target_frame: world

    # RViz 里目标点球体的显示尺寸
    marker_scale: 0.05

    # base_move_joint 负责整机沿过道方向移动
    "base_move_joint.source_axis": "x"
    "base_move_joint.zero_world_value": 0.0
    "base_move_joint.scale": 1.0
    "base_move_joint.min": 0.0
    "base_move_joint.max": 7.0
    "base_move_joint.speed": 0.50
    "base_move_joint.initial": 0.0

    # upper_lift_joint 负责上机械臂升降
    "upper_lift_joint.source_axis": "z"
    "upper_lift_joint.zero_world_value": 0.82
    "upper_lift_joint.scale": 1.0
    "upper_lift_joint.min": 0.0
    "upper_lift_joint.max": 0.23
    "upper_lift_joint.speed": 0.10
    "upper_lift_joint.initial": 0.0

    # upper_extend_joint 负责上机械臂前伸扶持
    "upper_extend_joint.source_axis": "y"
    "upper_extend_joint.zero_world_value": 0.35
    "upper_extend_joint.scale": -1.0
    "upper_extend_joint.min": -0.37
    "upper_extend_joint.max": 0.0
    "upper_extend_joint.speed": 0.08
    "upper_extend_joint.initial": 0.0

    # lower_lift_joint 负责下机械臂升降
    "lower_lift_joint.source_axis": "z"
    "lower_lift_joint.zero_world_value": 0.82
    "lower_lift_joint.scale": 1.0
    "lower_lift_joint.min": 0.0
    "lower_lift_joint.max": 0.23
    "lower_lift_joint.speed": 0.1
    "lower_lift_joint.initial": 0.0

    # lower_extend_joint 负责下机械臂伸出剪切
    "lower_extend_joint.source_axis": "y"
    "lower_extend_joint.zero_world_value": 0.35
    "lower_extend_joint.scale": -1.0
    "lower_extend_joint.min": -0.37
    "lower_extend_joint.max": 0.0
    "lower_extend_joint.speed": 0.08
    "lower_extend_joint.initial": 0.0

# 用 dataclass 描述关节映射关系与阶段动作
from dataclasses import dataclass
from tkinter import SEL
from typing import Dict, List, Optional

# ROS 2 消息与节点依赖
import rclpy
from geometry_msgs.msg import PoseStamped
from rclpy.node import Node
from sensor_msgs.msg import JointState
from visualization_msgs.msg import Marker

# 单个关节的映射配置：
# 将世界坐标中的某个轴映射到某个关节目标值
@dataclass
class JointMapping:
    name: str
    source_axis: str
    zero_world_value: float
    scale: float
    minimum: float
    maximum: float
    speed: float
    initial: float

@dataclass
class MotionPhase:
    name: str
    targets: Dict[str, float]
    pause_seconds: float = 0.0

# 核心控制节点：
# 订阅目标位姿，按顺序生成一组关节动作
class SimpleTargetFollower(Node):
    def __init__(self) -> None:
        super().__init__("simple_target_follower")

        # 基础运行参数
        self.declare_parameter("publish_rate_hz", 20.0)
        self.declare_parameter("target_topic", "/target_pose")
        self.declare_parameter("joint_state_topic", "/joint_states")
        self.declare_parameter("target_marker_topic", "/target_marker")
        self.declare_parameter("expected_target_frame", "world")
        self.declare_parameter("marker_scale", 0.05)
        self.declare_parameter("phase_tolerance", 0.002)
        self.declare_parameter("release_pause_seconds", 2.0)
        self.declare_parameter("cut_pause_seconds", 2.0)

        # 每个关节的默认映射与约束
        joint_defaults = {
            "base_move_joint": {
                "source_axis": "x",
                "zero_world_value": 0.0,
                "scale": 1.0,
                "min": 0.0,
                "max": 7.0,
                "speed": 0.60,
                "initial": 0.0,
            },
            "upper_lift_joint": {
                "source_axis": "z",
                "zero_world_value": 0.82,
                "scale": 1.0,
                "min": 0.0,
                "max": 0.23,
                "speed": 0.10,
                "initial": 0.0,
            },
            "upper_extend_joint": {
                "source_axis": "y",
                "zero_world_value": 0.35,
                "scale": -1.0,
                "min": -0.37,
                "max": 0.0,
                "speed": 0.08,
                "initial": 0.0,
            },
            "lower_lift_joint": {
                "source_axis": "z",
                "zero_world_value": 0.82,
                "scale": 1.0,
                "min": 0.0,
                "max": 0.23,
                "speed": 0.10,
                "initial": 0.0,
            },
            "lower_extend_joint": {
                "source_axis": "y",
                "zero_world_value": 0.35,
                "scale": -1.0,
                "min": -0.37,
                "max": 0.0,
                "speed": 0.08,
                "initial": 0.0,
            },
        }

        # 读取参数并转成 JointMapping 结构，便于后续统一计算
        self.joint_mappings: List[JointMapping] = []
        for joint_name, defaults in joint_defaults.items():
            self.declare_parameter(f"{joint_name}.source_axis", defaults["source_axis"])
            self.declare_parameter(f"{joint_name}.zero_world_value", defaults["zero_world_value"])
            self.declare_parameter(f"{joint_name}.scale", defaults["scale"])
            self.declare_parameter(f"{joint_name}.min", defaults["min"])
            self.declare_parameter(f"{joint_name}.max", defaults["max"])
            self.declare_parameter(f"{joint_name}.speed", defaults["speed"])
            self.declare_parameter(f"{joint_name}.initial", defaults["initial"])

            self.joint_mappings.append(
                JointMapping(
                    name=joint_name,
                    source_axis=str(self.get_parameter(f"{joint_name}.source_axis").value),
                    zero_world_value=float(self.get_parameter(f"{joint_name}.zero_world_value").value),
                    scale=float(self.get_parameter(f"{joint_name}.scale").value),
                    minimum=float(self.get_parameter(f"{joint_name}.min").value),
                    maximum=float(self.get_parameter(f"{joint_name}.max").value),
                    speed=float(self.get_parameter(f"{joint_name}.speed").value),
                    initial=float(self.get_parameter(f"{joint_name}.initial").value),
                )
            )

        # 将常用参数缓存到成员变量
        self.publish_rate_hz = float(self.get_parameter("publish_rate_hz").value)
        self.expected_target_frame = str(self.get_parameter("expected_target_frame").value)
        self.marker_scale = float(self.get_parameter("marker_scale").value)
        self.phase_tolerance = float(self.get_parameter("phase_tolerance").value)
        self.release_pause_seconds = float(self.get_parameter("release_pause_seconds").value)
        self.cut_phase_seconds = float(self.get_parameter("cut_pause_seconds").value)

        # 按名称建立索引，便于后续快速访问关节配置
        self.mapping_by_name: Dict[str, JointMapping] = {
            mapping.name: mapping for mapping in self.joint_mappings
        }

        # current_positions 记录当前状态，target_positions 记录阶段目标
        self.current_positions: Dict[str, float] = {
            mapping.name: mapping.initial for mapping in self.joint_mappings
        }
        self.target_positions: Dict[str, float] = dict(self.current_positions)

        # 阶段队列状态
        self.sequence_active = False
        self.phase_queue: List[MotionPhase] = []
        self.phase_index = -1
        self.phase_hold_until: Optional[float] = None
        self.phase_logged_complete = False

        target_topic = str(self.get_parameter("target_topic").value)
        joint_state_topic = str(self.get_parameter("joint_state_topic").value)
        marker_topic = str(self.get_parameter("target_marker_topic").value)

        # 输入目标位姿，输出 joint_states 和 target_marker
        self.create_subscription(PoseStamped, target_topic,  self._target_callback, 10)
        self.joint_state_publisher = self.create_publisher(JointState, joint_state_topic, 10)
        self.marker_publisher = self.create_publisher(Marker, marker_topic, 10)
        self.create_timer(1.0 / self.publish_rate_hz, self._timer_callback)

    # 收到新目标后，先做合法性检查，再生成本轮采收阶段队列
    def _target_callback(self, message: PoseStamped) -> None:
        frame_id = message.header.frame_id or self.expected_target_frame
        if frame_id != self.expected_target_frame:
            return

        if self.sequence_active:
            return

        coordinates = {
            "x": float(message.pose.position.x),
            "y": float(message.pose.position.y),
            "z": float(message.pose.position.z),
        }
        joint_targets = self._compute_joint_targets(coordinates)
        self._build_phase_queue(joint_targets)
        self._publish_target_marker(message)
        self._advance_phase()

    # 在 RViz 中发布目标点 Marker，方便观察目标位置
    def _publish_target_marker(self, message: PoseStamped) -> None:
        marker = Marker()
        marker.header = message.header
        marker.ns = "target"
        marker.id = 0
        marker.type = Marker.SPHERE
        marker.action = Marker.ADD
        marker.pose = message.pose
        marker.scale.x = self.marker_scale
        marker.scale.y = self.marker_scale
        marker.scale.z = self.marker_scale
        marker.color.r = 0.95
        marker.color.g = 0.1
        marker.color.b = 0.1
        marker.color.a = 1.0
        self.marker_publisher.publish(marker)

    # 定时器负责推进阶段状态，并按速度限制逐步逼近目标位置
    def _timer_callback(self) -> None:
        dt = 1.0 / self.publish_rate_hz

        if self.sequence_active:
            self._update_phase_progress()

        # 按各关节速度上限平滑逼近目标，而不是一步跳到位
        for mapping in self.joint_mappings:
            current = self.current_positions[mapping.name]
            target = self.target_positions[mapping.name]
            max_step = mapping.speed * dt
            delta = target - current
            if abs(delta) <= max_step:
                self.current_positions[mapping.name] = target
            else:
                direction = 1.0 if delta > 0.0 else -1.0
                self.current_positions[mapping.name] = current + max_step * direction

        # 将最新关节位置发布给 robot_state_publisher / RViz
        message = JointState()
        message.header.stamp = self.get_clock().now().to_msg()
        message.name = [mapping.name for mapping in self.joint_mappings]
        message.position = [self.current_positions[mapping.name] for mapping in self.joint_mappings]
        self.joint_state_publisher.publish(message)

    # 根据 x / y / z 坐标映射出各个关节的目标值，并做限位裁剪
    def _compute_joint_targets(self, coordinates: Dict[str, float]) -> Dict[str, float]:
        joint_targets: Dict[str, float] = {}
        for mapping in self.joint_mappings:
            raw_value = mapping.scale * (coordinates[mapping.source_axis] - mapping.zero_world_value)
            lower = min(mapping.minimum, mapping.maximum)
            upper = max(mapping.minimum, mapping.maximum)
            joint_targets[mapping.name] = min(max(raw_value, lower), upper)
        return joint_targets

    # 回到初始姿态，但保留底盘当前对齐后的 x 位置
    def _home_targets_with_base(self, base_value: float) -> Dict[str, float]:
        targets = {mapping.name: mapping.initial for mapping in self.joint_mappings}
        targets["base_move_joint"] = base_value
        return targets

    # 生成完整采收序列：
    # 对准 -> 升降对齐 -> 上臂扶持 -> 下臂剪切 -> 回撤 -> 复位
    def _build_phase_queue(self, joint_targets: Dict[str, float]) -> None:
        base_target = joint_targets["base_move_joint"]
        home_targets = self._home_targets_with_base(base_target)

        base_align = dict(home_targets)
        base_align["base_move_joint"] = base_target

        lift_align = dict(base_align)
        lift_align["upper_lift_joint"] = joint_targets["upper_lift_joint"]
        lift_align["lower_lift_joint"] = joint_targets["lower_lift_joint"]

        upper_hold = dict(lift_align)
        upper_hold["upper_extend_joint"] = joint_targets["upper_extend_joint"]

        lower_cut = dict(upper_hold)
        lower_cut["lower_extend_joint"] = joint_targets["lower_extend_joint"]

        lower_retract = dict(lower_cut)
        lower_retract["lower_extend_joint"] = self.mapping_by_name["lower_extend_joint"].initial

        upper_lift_raise = dict(lower_retract)
        upper_lift_raise["upper_lift_joint"] = max(
            self.mapping_by_name["upper_lift_joint"].minimum,
            self.mapping_by_name["upper_lift_joint"].maximum,
        )

        upper_retract = dict(upper_lift_raise)
        upper_retract["upper_extend_joint"] = self.mapping_by_name["upper_extend_joint"].initial

        lift_reset = dict(home_targets)
        lift_reset["base_move_joint"] = base_target

        self.phase_queue = [
            MotionPhase("base_align", base_align),
            MotionPhase("lift_align", lift_align),
            MotionPhase("upper_hold_extend", upper_hold),
            MotionPhase("lower_cut_extend", lower_cut, pause_seconds=self.cut_phase_seconds),
            MotionPhase("lower_retract", lower_retract),
            MotionPhase("upper_lift_raise", upper_lift_raise),
            MotionPhase("upper_retract", upper_retract, pause_seconds=self.release_pause_seconds),
            MotionPhase("lift_reset", lift_reset),
        ]
        self.phase_index = -1
        self.phase_hold_until = None
        self.phase_logged_complete = False
        self.sequence_active = True

    # 进入下一个阶段，并把该阶段目标写入 target_positions
    def _advance_phase(self) -> None:
        self.phase_index += 1
        self.phase_hold_until = None
        self.phase_logged_complete = False

        if self.phase_index >= len(self.phase_queue):
            self.sequence_active = False
            self.phase_queue = []
            return

        phase = self.phase_queue[self.phase_index]
        self.target_positions = dict(phase.targets)

    # 判断当前阶段是否到位；如设置了 pause_seconds，则先等待再切换阶段
    def _update_phase_progress(self) -> None:
        if not self.sequence_active or self.phase_index >= len(self.phase_queue):
            return

        phase = self.phase_queue[self.phase_index]
        if self._targets_reached(phase.targets):
            if phase.pause_seconds > 0.0:
                if self.phase_hold_until is None:
                    self.phase_hold_until = self.get_clock().now().nanoseconds / 1e9 + phase.pause_seconds
                    return
                if self.get_clock().now().nanoseconds / 1e9 < self.phase_hold_until:
                    return
            self._advance_phase()

    # 使用 phase_tolerance 判断当前关节组是否已经达到目标
    def _targets_reached(self, targets: Dict[str, float]) -> bool:
        return all(
            abs(self.current_positions[joint_name] - target_value) <= self.phase_tolerance
            for joint_name, target_value in targets.items()
        )

# ROS 2 节点标准入口
def main() -> None:
    rclpy.init()
    node = SimpleTargetFollower()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

这个 Launch 文件负责装配整套演示链路：加载 URDF，启动 simple_target_follower 和 robot_state_publisher， 并通过 use_rviz 开关控制 RViz2 是否在 5 秒后延迟启动。

# Launch 文件：负责把规划节点、机器人状态发布和 RViz 串起来
from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, TimerAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    # 获取两个包的 share 目录，方便读取 urdf / yaml / rviz 配置
    description_share = Path(get_package_share_directory("carnation_description"))
    planning_share = Path(get_package_share_directory("carnation_planning"))

    # 预设模型、RViz 配置和控制参数文件
    default_model = description_share / "urdf" / "carnation_robot_v0.urdf"
    default_rviz_config = description_share / "config" / "display.rviz"
    default_planner_config = planning_share / "config" / "simple_target_follower.yaml"

    # robot_state_publisher 直接读取 URDF 文本
    robot_description = {"robot_description": default_model.read_text(encoding="utf-8")}
    use_rviz = LaunchConfiguration("use_rviz")

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "use_rviz",
                default_value="true",
                description="Start RViz with the default robot model config.",
            ),
            # 控制节点：订阅 /target_pose，输出 /joint_states 与 /target_marker
            Node(
                package="carnation_planning",
                executable="simple_target_follower",
                name="simple_target_follower",
                parameters=[str(default_planner_config)],
                output="screen",
            ),
            # 根据 joint_states 和 URDF 计算整棵 TF 树
            Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                name="robot_state_publisher",
                parameters=[robot_description],
                output="screen",
            ),
            # 延迟启动 RViz，给前面两个节点留出初始化时间
            TimerAction(
                period=5.0,
                actions=[
                    Node(
                        package="rviz2",
                        executable="rviz2",
                        name="rviz2",
                        arguments=["-d", str(default_rviz_config)],
                        condition=IfCondition(use_rviz),
                        output="screen",
                    )
                ],
            ),
        ]
    )

这份 Launch 用于单独显示机器人模型。它支持在 joint_state_publisher_gui 与普通 joint_state_publisher 间切换，并可直接拉起 RViz2 查看 URDF 和 TF。

# Launch 文件：单独用于显示机器人模型与 TF 结构
from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    # 获取模型和 RViz 配置文件
    package_share = Path(get_package_share_directory("carnation_description"))
    default_model = package_share / "urdf" / "carnation_robot_v0.urdf"
    default_rviz_config = package_share / "config" / "display.rviz"

    # robot_state_publisher 读取 URDF 文本并发布到 /robot_description
    robot_description = {"robot_description": default_model.read_text(encoding="utf-8")}

    # 两个开关：
    # 1. 是否用 GUI 手动调关节
    # 2. 是否启动 RViz
    use_joint_state_gui = LaunchConfiguration("use_joint_state_gui")
    use_rviz = LaunchConfiguration("use_rviz")

    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "use_joint_state_gui",
                default_value="true",
                description="Start joint_state_publisher_gui instead of the CLI publisher.",
            ),
            DeclareLaunchArgument(
                "use_rviz",
                default_value="true",
                description="Start RViz with the default robot model config.",
            ),
            # GUI 版本的 joint_state_publisher，适合手动拖关节调姿态
            Node(
                package="joint_state_publisher_gui",
                executable="joint_state_publisher_gui",
                name="joint_state_publisher_gui",
                arguments=[str(default_model)],
                condition=IfCondition(use_joint_state_gui),
            ),
            # 非 GUI 版本，适合无界面环境
            Node(
                package="joint_state_publisher",
                executable="joint_state_publisher",
                name="joint_state_publisher",
                arguments=[str(default_model)],
                condition=UnlessCondition(use_joint_state_gui),
            ),
            # 依据 URDF + joint_states 发布各 link 的坐标关系
            Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                name="robot_state_publisher",
                parameters=[robot_description],
                output="screen",
            ),
            # 打开 RViz 进行模型、TF 与 Marker 可视化
            Node(
                package="rviz2",
                executable="rviz2",
                name="rviz2",
                arguments=["-d", str(default_rviz_config)],
                condition=IfCondition(use_rviz),
                output="screen",
            ),
        ]
    )