当前位置：首页 > news >正文

DNN 预测手术机器人姿态并做补偿包工程样本（2025.09）

news 2025/11/6 7:04:40

ROS 2 Python 包（rclpy）工程样本，包含：目录结构、package.xml、CMakeLists.txt、setup.py、示例节点源码、参数文件与 launch。默认使用标准消息类型（无需自定义 .msg）：

输入
- /camera/image — sensor_msgs/Image
- /joint_states — sensor_msgs/JointState
- /imu/data — sensor_msgs/Imu
- /wrench — geometry_msgs/WrenchStamped（可选）
- /x_star — geometry_msgs/PoseStamped（参考位姿）
输出
- /pose_est — geometry_msgs/PoseWithCovarianceStamped（DNN 姿态与协方差）
- /delta_x — geometry_msgs/TwistStamped（RL 小幅修正）
- /cartesian_cmd — geometry_msgs/TwistStamped（控制器任务空间速度指令）
- 或 /joint_group_vel_controller/commands — std_msgs/Float64MultiArray（若你使用关节速度控制器）

目录结构

surgical_robot_control/
├── package.xml
├── CMakeLists.txt
├── setup.cfg
├── setup.py
├── resource/
│   └── surgical_robot_control
├── surgical_robot_control/
│   ├── __init__.py
│   ├── utils.py
│   ├── posenet_node.py
│   ├── rl_policy_node.py
│   ├── controller_node.py
│   ├── safety_projector.py
│   └── typing_stub.pyi
├── launch/
│   └── surgical_control.launch.py
├── params/
│   └── default.yaml
├── models/
│   ├── posenet.engine         # 可放 TensorRT 引擎（可选）
│   └── posenet.onnx           # 或 ONNX（可选）
└── policies/└── ppo_policy.pt          # RL 权重文件（可选）

放到你的 ROS 2 工作区 src/ 下（例如 ~/ros2_ws/src/），然后 colcon build。

package.xml

<?xml version="1.0"?>
<package format="3"><name>surgical_robot_control</name><version>0.1.0</version><description>Pose DNN + RL correction + task-space controller for surgical robot (ROS 2).</description><maintainer email="you@example.com">Your Name</maintainer><license>Apache-2.0</license><buildtool_depend>ament_cmake</buildtool_depend><buildtool_depend>ament_python</buildtool_depend><exec_depend>rclpy</exec_depend><exec_depend>sensor_msgs</exec_depend><exec_depend>geometry_msgs</exec_depend><exec_depend>std_msgs</exec_depend><exec_depend>std_srvs</exec_depend><exec_depend>builtin_interfaces</exec_depend><exec_depend>cv_bridge</exec_depend><exec_depend>image_transport</exec_depend><exec_depend>message_filters</exec_depend><!-- 如需实时关节控制，可引入 control_msgs/controller_manager 等 --><export><build_type>ament_cmake</build_type></export>
</package>

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(surgical_robot_control)find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)# 纯 Python 包，无需编译
ament_python_install_package(${PROJECT_NAME})install(DIRECTORY launch params models policies resourceDESTINATION share/${PROJECT_NAME}PATTERN "*.pt" EXCLUDE  # 可按需包含/排除权重
)install(PROGRAMSsurgical_robot_control/posenet_node.pysurgical_robot_control/rl_policy_node.pysurgical_robot_control/controller_node.pysurgical_robot_control/safety_projector.pyDESTINATION lib/${PROJECT_NAME}
)ament_package()

setup.py

from setuptools import setup
package_name = 'surgical_robot_control'setup(name=package_name,version='0.1.0',packages=[package_name],data_files=[('share/ament_index/resource_index/packages',['resource/' + package_name]),('share/' + package_name, ['package.xml']),('share/' + package_name + '/launch', ['launch/surgical_control.launch.py']),('share/' + package_name + '/params', ['params/default.yaml']),],install_requires=['setuptools'],zip_safe=True,maintainer='Your Name',maintainer_email='you@example.com',description='Pose DNN + RL correction + controller nodes.',license='Apache-2.0',entry_points={'console_scripts': ['posenet_node = surgical_robot_control.posenet_node:main','rl_policy_node = surgical_robot_control.rl_policy_node:main','controller_node = surgical_robot_control.controller_node:main','safety_projector = surgical_robot_control.safety_projector:main',],},
)

params/default.yaml

surgical_robot_control:ros__parameters:# 通用frame_id: "base_link"ee_frame_id: "tool_tip"use_wrench: truerate_hz: 200# PoseNetposenet:engine_path: "$(find-pkg-share surgical_robot_control)/models/posenet.engine"onnx_path:   "$(find-pkg-share surgical_robot_control)/models/posenet.onnx"img_size: [256, 256]use_fp16: truepublish_covariance: true# RL Policyrl:policy_path: "$(find-pkg-share surgical_robot_control)/policies/ppo_policy.pt"action_limit: 0.002           # m / rad per stepjerk_penalty_gain: 0.0# Safety Projector (QP)safety:enable: truemax_lin_vel: 0.01             # m/smax_ang_vel: 0.10             # rad/sforce_limit: 5.0              # N, 仅当 use_wrench=trueqp_timeout_ms: 2# Controller (任务空间伺服)controller:kp_lin: 4.0kp_ang: 3.0publish_joint_vel: false      # 若为 true，发布 joint velocityjoint_vel_topic: "/joint_group_vel_controller/commands"

launch/surgical_control.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import ThisLaunchFileDir
from ament_index_python.packages import get_package_share_directory
import osdef generate_launch_description():pkg_share = get_package_share_directory('surgical_robot_control')params = os.path.join(pkg_share, 'params'<