1.创建文件 trajectory_receiver.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseArray, Pose
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
from std_msgs.msg import String
import sys
import threading
class TrajectoryReceiver:
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('trajectory_receiver', anonymous=True)
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self.group = MoveGroupCommander("firefighter_arm") # 修改为在 firefighter.srdf 文件中找到的规划组名
self.group.set_planning_time(10)
self.trajectory_subscriber = rospy.Subscriber("robot_arm_trajectory", PoseArray, self.trajectory_callback)
self.waypoints = []
self.lock = threading.Lock()
def trajectory_callback(self, msg):
rospy.loginfo("Received trajectory with %d points", len(msg.poses))
with self.lock:
self.waypoints = list(msg.poses)
threading.Thread(target=self.process_trajectory).start()
def process_trajectory(self):
with self.lock:
waypoints = self.waypoints
if not waypoints:
return
rospy.loginfo("Processing trajectory with %d points", len(waypoints))
plan, fraction = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.group.execute(plan, wait=True)
else:
rospy.logwarn("Path planning failed with only %.2f%% success.", fraction * 100)
def start(self):
rospy.spin()
if __name__ == "__main__":
try:
traj_receiver = TrajectoryReceiver()
traj_receiver.start()
except rospy.ROSInterruptException:
pass
finally:
roscpp_shutdown()
2.在 CMakeLists.txt
中
find_package(catkin REQUIRED COMPONENTS
rospy
moveit_commander
geometry_msgs
std_msgs
)
catkin_package(
CATKIN_DEPENDS rospy moveit_commander geometry_msgs std_msgs
)
catkin_install_python(PROGRAMS
src/trajectory_receiver.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)