marker_control.py 文件源码

def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        # Publishes Cartesian goals
        self.pub_command = rospy.Publisher("/cartesian_command", geometry_msgs.msg.Transform, 
                                           queue_size=1)

        # Publishes Redundancy goals
        self.pub_red = rospy.Publisher("/redundancy_command", Float32, queue_size=1)

        # Publisher to set robot position
        self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1)

        #This is where we hold the most recent joint transforms
        self.joint_transforms = []
        self.x_current = tf.transformations.identity_matrix()
        self.R_base = tf.transformations.identity_matrix()

        #Create "Interactive Marker" that we can manipulate in RViz
        self.init_marker()
        self.ee_tracking = 0
        self.red_tracking = 0
        self.q_current = []

        self.x_target = tf.transformations.identity_matrix()
        self.q0_desired = 0

        self.mutex = Lock()        
        self.timer = rospy.Timer(rospy.Duration(0.3), self.timer_callback)

        #Subscribes to information about what the current joint values are.
        rospy.Subscriber("joint_states", JointState, self.joint_callback)

    #Resets the robot to a known pose