pose_controller.py 文件源码

def executeJointTrajectoryAction(self, goal):
        rospy.loginfo("JointTrajectory action executing");

        names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)

        rospy.logdebug("Received trajectory for joints: %s times: %s", str(names), str(times))
        rospy.logdebug("Trajectory angles: %s", str(angles))

        task_id = None
        running = True
        #Wait for task to complete
        while running and not self.jointTrajectoryServer.is_preempt_requested() and not rospy.is_shutdown():
            #If we haven't started the task already...
            if task_id is None:
                # ...Start it in another thread (thanks to motionProxy.post)
                task_id = self.motionProxy.post.angleInterpolation(names, angles, times, (goal.relative==0))

            #Wait for a bit to complete, otherwise check we can keep running
            running = self.motionProxy.wait(task_id, self.poll_rate)

        # If still running at this point, stop the task
        if running and task_id:
            self.motionProxy.stop( task_id )

        jointTrajectoryResult = JointTrajectoryResult()
        jointTrajectoryResult.goal_position.header.stamp = rospy.Time.now()
        jointTrajectoryResult.goal_position.position = self.motionProxy.getAngles(names, True)
        jointTrajectoryResult.goal_position.name = names

        if not self.checkJointsLen(jointTrajectoryResult.goal_position):
            self.jointTrajectoryServer.set_aborted(jointTrajectoryResult)
            rospy.logerr("JointTrajectory action error in result: sizes mismatch")

        elif running:
            self.jointTrajectoryServer.set_preempted(jointTrajectoryResult)
            rospy.logdebug("JointTrajectory preempted")

        else:
            self.jointTrajectoryServer.set_succeeded(jointTrajectoryResult)
            rospy.loginfo("JointTrajectory action done")