视频讲解

创建 ROS 2 包

进入工作空间的 src 目录，然后创建一个新的 Python 包：

ros2 pkg create --build-type ament_python panda_joint_control --dependencies rclpy control_msgs trajectory_msgs

依赖于 rclpy、control_msgs 和 trajectory_msgs

编写 Python 节点代码

在 panda_joint_control 包的 panda_joint_control 子目录下创建一个名为 panda_joint_controller.py 的文件，并添加以下代码：

import rclpy
from rclpy.node import Node
from control_msgs.msg import JointTrajectoryControllerState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import sys
import tty
import termiosclass PandaJointController(Node):def __init__(self):super().__init__('panda_joint_controller')# 创建发布者，发布到 "/panda_arm_controller/joint_trajectory" 话题self.publisher_ = self.create_publisher(JointTrajectory, '/panda_arm_controller/joint_trajectory', 10)# 定义关节名称列表self.joint_names = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7']# 初始化关节位置self.joint_positions = [0.0] * 7# 定义每个关节位置的递增步长self.step = 0.1def get_key(self):# 获取终端输入的按键settings = termios.tcgetattr(sys.stdin)try:tty.setraw(sys.stdin.fileno())key = sys.stdin.read(1)finally:termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keydef send_trajectory(self):# 创建 JointTrajectory 消息trajectory_msg = JointTrajectory()trajectory_msg.joint_names = self.joint_names# 创建 JointTrajectoryPoint 并设置目标位置等信息point = JointTrajectoryPoint()point.positions = self.joint_positions# 设置运动时间point.time_from_start = rclpy.duration.Duration(seconds=1).to_msg()# 将点添加到轨迹消息中trajectory_msg.points.append(point)# 发布轨迹消息self.publisher_.publish(trajectory_msg)self.get_logger().info('Sent joint trajectory command')def run(self):while rclpy.ok():key = self.get_key()if key == '-':self.minus_pressed = Trueelif key in ['1', '2', '3', '4', '5', '6', '7']:index = int(key) - 1if self.minus_pressed:self.joint_positions[index] -= self.stepself.minus_pressed = Falseelse:self.joint_positions[index] += self.stepself.send_trajectory()elif key == '\x03':  # Ctrl+C 退出breakdef main(args=None):rclpy.init(args=args)panda_joint_controller = PandaJointController()panda_joint_controller.run()panda_joint_controller.destroy_node()rclpy.shutdown()if __name__ == '__main__':main()

修改 setup.py 文件

打开 panda_joint_control 包的 setup.py 文件，在 entry_points 部分添加以下内容：

entry_points={'console_scripts': ['panda_joint_controller = panda_joint_control.panda_joint_controller:main',],
},

编译和运行

colcon build --packages-select panda_joint_control
source install/setup.bash
ros2 run panda_joint_control panda_joint_controller

按下按键 1 - 7 来控制 Panda 机械臂相应关节的位置递增，按下 Ctrl + C 可以退出程序