unity-ros数据传输

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}
)