撰写脚本，通过发布/joint_states话题改变机器人在Rviz中的关节角度

提问

为我写一个改变关节base_spherical_center_high_joint角度的python脚本吧。适用于ROS2的humble

回答

下面是一个适用于 ROS 2 Humble 的 Python 脚本，它会以指定频率持续发布 base_spherical_center_high_joint 的角度到 /joint_states，从而驱动 robot_state_publisher 正常广播 TF。

---

✅ publish_joint_angle.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from builtin_interfaces.msg import Time
import math

class JointPublisher(Node):
    def __init__(self):
        super().__init__('joint_state_publisher_node')

        self.publisher_ = self.create_publisher(JointState, '/joint_states', 10)

        # Timer: 10Hz
        self.timer = self.create_timer(0.1, self.publish_joint_state)

        # Desired joint position (radians)
        self.joint_position = 1.5708  # 90 degrees

    def publish_joint_state(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = ['base_spherical_center_high_joint']
        msg.position = [self.joint_position]
        self.publisher_.publish(msg)
        self.get_logger().info(f'Publishing: {msg.name[0]} = {self.joint_position:.4f} rad')

def main(args=None):
    rclpy.init(args=args)
    node = JointPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

✅ 使用说明：

1. 保存为 publish_joint_angle.py。
2. 给脚本加执行权限：

chmod +x publish_joint_angle.py

3. 运行脚本（建议在 ROS2 工作空间中）：

ros2 run <your_package_name> publish_joint_angle

或在独立目录中运行：

python3 publish_joint_angle.py

---

需要我帮你把它打包成 ROS2 可执行节点（添加到 setup.py 和 entry_points）吗？

继续提问

可以帮我添加。直接在这个基础上修改：
setup(
name=package_name,
version=‘0.0.0’,
packages=find_packages(exclude=[‘test’]),
# data_files=[
# (‘share/ament_index/resource_index/packages’,
# [‘resource/’ + package_name]),
# (‘share/’ + package_name, [‘package.xml’]),
# ],
data_files = data_files_list,
install_requires=[‘setuptools’],
zip_safe=True,
maintainer=‘usv-6081’,
maintainer_email=‘usv-6081@todo.todo’,
description=‘TODO: Package description’,
license=‘TODO: License declaration’,
tests_require=[‘pytest’],
entry_points={
‘console_scripts’: [
‘hello_node = control_planner.hello_node:main’,
‘usv_Simulation = control_planner.usvSimV2:main’,
‘keyboard_interaction = control_planner.keyboard_interaction:main’,
],
},
)

回答

当然可以！你只需要将新的脚本添加到 entry_points['console_scripts'] 列表中，并确保你的 Python 文件（如 publish_joint_angle.py）位于包内模块路径 control_planner/ 下，并包含 main() 函数。

---

✅ 修改后的 setup.py 示例（新增 publish_joint_angle 入口）：

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=data_files_list,
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='usv-6081',
    maintainer_email='usv-6081@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'hello_node = control_planner.hello_node:main',
            'usv_Simulation = control_planner.usvSimV2:main',
            'keyboard_interaction = control_planner.keyboard_interaction:main',
            'publish_joint_angle = control_planner.publish_joint_angle:main',  # 新增这一行
        ],
    },
)

---

✅ 注意事项：

1. publish_joint_angle.py 文件要放在 control_planner/ 目录中（和其他 .py 文件一样）。

2. 文件中必须包含：

   if __name__ == '__main__':
       main()
   

   并有 main() 函数定义。

3. 别忘了在工作区根目录执行构建命令：

   colcon build
   source install/setup.bash
   

---

然后你就可以直接运行这个脚本：

ros2 run control_planner publish_joint_angle

需要我帮你检查 publish_joint_angle.py 是否需要加入 __init__.py 或补充模块结构吗？

结果

成功