import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from trac_ik_python.trac_ik import IK
# 初始化 ROS 节点
rospy.init_node('dofbot_ik_node')
# 创建一个发布 JointState 的 ROS 发布器
joint_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
# 创建 trac_ik 的对象
ik_solver = IK('base_link', 'arm_end_link', timeout=0.1)
# 设置目标位置
target_position = [1.0, 0.0, 1.0] # 以米为单位
# 获取当前机械臂关节角度
current_joint_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# 使用 trac_ik 计算目标关节角度
joint_angles = ik_solver.get_ik(current_joint_angles, target_position[0], target_position[1], target_position[2], 0, 0, 0)
# 发布关节角度消息
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
joint_state.position = joint_angles
joint_pub.publish(joint_state)