Python SimpleActionServer.publish_feedback方法代码示例

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]
class PipolFollower():
    def __init__(self):
        
        rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'")
        self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction, 
                                      execute_cb = self.execute_cb,
                                      preempt_callback = self.preempt_cb, 
                                      auto_start = False)
        rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
        self._as.start()
        
    def execute_cb(self, goal):
        print "goal is: " + str(goal)
        # helper variables
        success = True
        
        # start executing the action
        for i in xrange(1, goal.order):
          # check that preempt has not been requested by the client
          if self._as.is_preempt_requested():
            rospy.loginfo('%s: Preempted' % self._action_name)
            self._as.set_preempted()
            success = False
            break
          self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
          # publish the feedback
          self._as.publish_feedback(self._feedback)
          # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
          r.sleep()
          
        if success:
          self._result.sequence = self._feedback.sequence
          rospy.loginfo('%s: Succeeded' % self._action_name)
          self._as.set_succeeded(self._result)  

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]

#.........这里部分代码省略.........
        # get parameters from launch file
        self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
        # action server based on use_obstacle_avoidance
        if self.use_obstacle_avoidance == False:
            self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
        else:
            self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)

        self.goal_client.wait_for_server()

        # other fields
        self.goal_index = 0
        self.executePathGoal = None
        self.executePathResult = ExecutePathResult()

    def handle_path(self, paramExecutePathGoal):
        '''
        Action server callback to handle following path in succession
        '''
        rospy.loginfo('handle_path')

        self.goal_index = 0
        self.executePathGoal = paramExecutePathGoal
        self.executePathResult = ExecutePathResult()

        if self.executePathGoal is not None:
            self.visualization_publisher.publish(self.executePathGoal.path)

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            if not self.path_server.is_active():
                return

            if self.path_server.is_preempt_requested():
                rospy.loginfo('Preempt requested...')
                # Stop bug2
                self.goal_client.cancel_goal()
                # Stop path_server
                self.path_server.set_preempted()
                return

            if self.goal_index < len(self.executePathGoal.path.poses):
                moveto_goal = MoveToGoal()
                moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
                self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
                                           feedback_cb=self.handle_goal_preempt)
                # Wait for result
                while self.goal_client.get_result() is None:
                    if self.path_server.is_preempt_requested():
                        self.goal_client.cancel_goal()
            else:
                rospy.loginfo('Done processing goals')
                self.goal_client.cancel_goal()
                self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
                return

            rate.sleep()
        self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')


    def handle_goal(self, state, result):
        '''
        Handle goal events (succeeded, preempted, aborted, unreachable, ...)
        Send feedback message
        '''
        feedback = ExecutePathFeedback()
        feedback.pose = self.executePathGoal.path.poses[self.goal_index]

        # state is GoalStatus message as shown here:
        # http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
        if state == GoalStatus.SUCCEEDED:
            rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
            self.executePathResult.visited.append(True)
            feedback.reached = True
        else:
            rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
            self.executePathResult.visited.append(False)
            feedback.reached = False

            if not self.executePathGoal.skip_unreachable:
                rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
                # Stop bug2
                self.goal_client.cancel_goal()
                # Stop path_server
                self.path_server.set_succeeded(self.executePathResult,
                                             'Unreachable goal in path. Done processing goals.')
                #self.path_server.set_preempted()
                #return

        self.path_server.publish_feedback(feedback)

        self.goal_index = self.goal_index + 1


    def handle_goal_preempt(self, state):
        '''
        Callback for goal_client to check for preemption from path_server
        '''
        if self.path_server.is_preempt_requested():
            self.goal_client.cancel_goal()

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]

#.........这里部分代码省略.........
        Create ros bag save directory
        If no bag name is provided, the current date/time is used as default.
        """
        # If directory param is not set, default to $HOME/bags/<date>
        default_dir = self.dir
        if default_dir is None:
            default_dir = os.path.join(os.environ['HOME'], 'bags')

        # if dated folder param is set to True, append current date to
        # directory
        if self.dated_folder is True:
            default_dir = os.path.join(default_dir, str(datetime.date.today()))
        # Split filename from directory
        bag_dir, bag_name = os.path.split(filename)
        bag_dir = os.path.join(default_dir, bag_dir)
        if not os.path.exists(bag_dir):
            os.makedirs(bag_dir)

        # Create default filename if only directory specified
        if bag_name == '':
            bag_name = self._get_default_filename()
        # Make sure filename ends in .bag, add otherwise
        if bag_name[-4:] != '.bag':
            bag_name = bag_name + '.bag'
        return os.path.join(bag_dir, bag_name)

    def start_bagging(self, req):
        """
        Dump all data in dictionary to bags, temporarily stops streaming
        during the bagging process, resumes streaming when over.
        If bagging is already false because of an active call to this service
        """
        result = BagOnlineResult()
        if self.streaming is False:
            result.status = 'Bag Request came in while bagging, priority given to prior request'
            result.success = False
            self._action_server.set_aborted(result)
            return
        bag = None
        try:
            self.streaming = False
            result.filename = self.get_bag_name(req.bag_name)
            bag = rosbag.Bag(result.filename, 'w')

            requested_seconds = req.bag_time

            selected_topics = req.topics.split()

            feedback = BagOnlineFeedback()
            total_messages = 0
            bag_topics = {}
            for topic, (time, subscribed) in self.subscriber_list.iteritems():
                if not subscribed:
                    continue
                # Exclude topics that aren't in topics service argument
                # If topics argument is empty string, include all topics
                if len(selected_topics) > 0 and topic not in selected_topics:
                    continue
                if len(self.topic_messages[topic]) == 0:
                    continue

                if req.bag_time == 0:
                    index = 0
                else:
                    index = self.get_time_index(topic, requested_seconds)
                total_messages += len(self.topic_messages[topic][index:])
                bag_topics[topic] = index
            if total_messages == 0:
                result.success = False
                result.status = 'no messages'
                self._action_server.set_aborted(result)
                self.streaming = True
                bag.close()
                return
            self._action_server.publish_feedback(feedback)
            msg_inc = 0
            for topic, index in bag_topics.iteritems():
                for msgs in self.topic_messages[topic][index:]:
                    bag.write(topic, msgs[1], t=msgs[0])
                    if msg_inc % 50 == 0:  # send feedback every 50 messages
                        feedback.progress = float(msg_inc) / total_messages
                        self._action_server.publish_feedback(feedback)
                    msg_inc += 1
                    # empty deque when done writing to bag
                self.topic_messages[topic].clear()
            feedback.progress = 1.0
            self._action_server.publish_feedback(feedback)
            bag.close()
        except Exception as e:
            result.success = False
            result.status = 'Exception while writing bag: ' + str(e)
            self._action_server.set_aborted(result)
            self.streaming = True
            if bag is not None:
                bag.close()
            return
        rospy.loginfo('Bag written to {}'.format(result.filename))
        result.success = True
        self._action_server.set_succeeded(result)
        self.streaming = True

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]

#.........这里部分代码省略.........
    def read_wrist_tilt(self, rotate_data):
        self.wrist_tilt = rotate_data
        self.has_latest_wrist_tilt = True


    def wait_for_latest_controller_states(self, timeout):
        self.has_latest_shoulder_pan = False
        self.has_latest_arm_tilt = False
        self.has_latest_elbow_tilt = False
        self.has_latest_wrist_pan = False
        self.has_latest_wrist_tilt = False
        r = rospy.Rate(100)
        start = rospy.Time.now()
        while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \
                self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \
                (rospy.Time.now() - start < timeout):
            r.sleep()


    def transform_target_point(self, point):
        rospy.loginfo("%s: Retrieving IK solutions", NAME)
        rospy.wait_for_service('smart_arm_ik_service', 10)
        ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK)
        resp = ik_service(point)
        if (resp and resp.success):
            return resp.solutions[0:4]
        else:
            raise Exception, "Unable to obtain IK solutions."


    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.result.success = True
        self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
                self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
        rospy.loginfo("%s: Executing move arm", NAME)
        
        # Initialize target joints
        target_joints = list()
        for i in range(self.JOINTS_COUNT):
            target_joints.append(0.0)
        
        # Retrieve target joints from goal
        if (len(goal.target_joints) > 0):
            for i in range(min(len(goal.target_joints), len(target_joints))):
                target_joints[i] = goal.target_joints[i] 
        else:
            try:
                # Convert target point to target joints (find an IK solution)
                target_joints = self.transform_target_point(goal.target_point)
            except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("%s: Aborted: IK Transform Failure", NAME)
                self.result.success = False
                self.server.set_aborted()
                return

        # Publish goal to controllers
        self.shoulder_pan_pub.publish(target_joints[0])
        self.arm_tilt_pub.publish(target_joints[1])
        self.elbow_tilt_pub.publish(target_joints[2])
        self.wrist_pan_pub.publish(target_joints[3])
        self.wrist_tilt_pub.publish(target_joints[4])
        
        # Initialize loop variables
        start_time = rospy.Time.now()

        while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD):
		
	        # Cancel exe if another goal was received (i.e. preempt requested)
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.result.success = False
                self.server.set_preempted()
                break

            # Publish current arm position as feedback
            self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
                    self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
            self.server.publish_feedback(self.feedback)
            
            # Abort if timeout
            current_time = rospy.Time.now()
            if (current_time - start_time > self.TIMEOUT_THRESHOLD):
                rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                self.result.success = False
                self.server.set_aborted()
                break

            r.sleep()

        if (self.result.success):
            rospy.loginfo("%s: Goal Completed", NAME)
            self.wait_for_latest_controller_states(rospy.Duration(2.0))
            self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
                    self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
            self.server.set_succeeded(self.result)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]

#.........这里部分代码省略.........
    def reset_head_position(self):
        self.head_pan_pub.publish(0.0)
        self.head_tilt_pub.publish(0.0)
        rospy.sleep(5)


    def wait_for_latest_controller_states(self, timeout):
        self.has_latest_pan = False
        self.has_latest_tilt = False
        r = rospy.Rate(100)
        start = rospy.Time.now()
        while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout):
            r.sleep()


    def transform_target_point(self, point):
        pan_target_frame = self.head_pan_frame
        tilt_target_frame = self.head_tilt_frame
        
        # Wait for tf info (time-out in 5 seconds)
        self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
        self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))

        # Transform target point to pan reference frame & retrieve the pan angle
        pan_target = self.tf.transformPoint(pan_target_frame, point)
        pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
        #rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
        #        NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)

        # Transform target point to tilt reference frame & retrieve the tilt angle
        tilt_target = self.tf.transformPoint(tilt_target_frame, point)
        tilt_angle = math.atan2(tilt_target.point.z,
                math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
        #rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
        #        NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)

        return [pan_angle, tilt_angle]


    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.result.success = True
        self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
        rospy.loginfo("%s: Executing move head", NAME)
        
        # Initialize target joints
        target_joints = list()
        for i in range(self.JOINTS_COUNT):
            target_joints.append(0.0)
        
        # Retrieve target joints from goal
        if (len(goal.target_joints) > 0):
            for i in range(min(len(goal.target_joints), len(target_joints))):
                target_joints[i] = goal.target_joints[i] 
        else:
            try:
                # Try to convert target point to pan & tilt angles
                target_joints = self.transform_target_point(goal.target_point)
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("%s: Aborted: Transform Failure", NAME)
                self.result.success = False
                self.server.set_aborted()
                return

	    # Publish goal command to controllers
        self.head_pan_pub.publish(target_joints[0])
        self.head_tilt_pub.publish(target_joints[1])

        # Initialize loop variables
        start_time = rospy.Time.now()

        while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD):
		
	        # Cancel exe if another goal was received (i.e. preempt requested)
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.result.success = False
                self.server.set_preempted()
                break

            # Publish current head position as feedback
            self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
            self.server.publish_feedback(self.feedback)
            
            # Abort if timeout
            current_time = rospy.Time.now()
            if (current_time - start_time > self.TIMEOUT_THRESHOLD):
                rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                self.result.success = False
                self.server.set_aborted()
                break

            r.sleep()
        
        if (self.result.success):
            rospy.loginfo("%s: Goal Completed", NAME)
            self.wait_for_latest_controller_states(rospy.Duration(2.0))
            self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
            self.server.set_succeeded(self.result)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]
class PickUpActionServer():

    def __init__(self):


        # Initialize new node
        rospy.init_node(NAME)#, anonymous=False)

        #initialize the clients to interface with 
        self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
        self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
        self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
        
        self.move_client.wait_for_server()
        self.arm_client.wait_for_server()
        self.gripper_client.wait_for_server()

        # Initialize tf listener (remove?)
        self.tf = tf.TransformListener()

        # Initialize erratic base action server
        self.result = SmartArmGraspResult()
        self.feedback = SmartArmGraspFeedback()
        self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)

        #define the offsets
    	# These offsets were determines expirmentally using the simulator
    	# They were tested using points stamped with /map
        self.xOffset = 0.025
        self.yOffset = 0.0
        self.zOffset = 0.12 #.05 # this does work!

        rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
        rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )

    def move_to(self, frame_id, position, orientation, vicinity=0.0):
        goal = ErraticBaseGoal()
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.header.frame_id = frame_id
        goal.target_pose.pose.position = position
        goal.target_pose.pose.orientation = orientation
        goal.vicinity_range = vicinity
        self.move_client.send_goal(goal)
        #print "going into loop"
        while (not self.move_client.wait_for_result(rospy.Duration(0.01))):
            # check for preemption
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.move_client.cancel_goal()
                return GoalStatus.PREEMPTED
        
        return self.move_client.get_state()

    #(almost) blatent copy / past from wubble_head_action.py.  Err, it's going to the wrong position
    def transform_target_point(self, point, frameId):
        
        #rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)

        # Wait for tf info (time-out in 5 seconds)
        self.tf.waitForTransform(frameId, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))

        # Transform target point & retrieve the pan angle
        return self.tf.transformPoint(frameId, point)

#UNUSED
    def move_base_feedback_cb(self, fb):
        self.feedback.base_position = fb.base_position
        if self.server.is_active():
            self.server.publish_feedback(self.feedback)

# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
    def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
        goal = SmartArmGoal()
        goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]

        self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
        self.arm_client.wait_for_goal_to_finish()

        while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
            # check for preemption
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.arm_client.cancel_goal()
                return GoalStatus.PREEMPTED

        return self.arm_client.get_state()

# This moves the wrist of the arm to the x, y, z coordinates
    def reach_at(self, frame_id, x, y, z):
	        
        goal = SmartArmGoal()
        goal.target_point = PointStamped()
        goal.target_point.header.frame_id = frame_id
        goal.target_point.point.x = x
        goal.target_point.point.y = y
        goal.target_point.point.z = z

       	rospy.loginfo("%s: Original point is '%s' [%f, %f, %f]", NAME, frame_id,\
		goal.target_point.point.x, goal.target_point.point.y, goal.target_point.point.z) 
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]
class AbstractHMIServer(object):
    """
    Abstract base class for a hmi client

    >>> class HMIServer(AbstractHMIServer):
    ...     def __init__(self):
    ...         pass
    ...     def _determine_answer(self, description, spec, choices):
    ...         return QueryResult()
    ...     def _set_succeeded(self, result):
    ...         print result

    >>> server = HMIServer()

    >>> from hmi_msgs.msg import QueryGoal
    >>> goal = QueryGoal(description='q', spec='spec', choices=[])

    >>> server._execute_cb(goal)
    raw_result: ''
    results: []

    >>> class HMIServer(AbstractHMIServer):
    ...     def __init__(self):
    ...         pass


    >>> server = HMIServer()
    Traceback (most recent call last):
    ...
    TypeError: Can't instantiate abstract class HMIServer with abstract methods _determine_answer

    """
    __metaclass__ = ABCMeta

    def __init__(self, name):
        self._action_name = name
        self._server = SimpleActionServer(name, QueryAction,
                                         execute_cb=self._execute_cb, auto_start=False)
        self._server.start()
        rospy.loginfo('HMI server started on "%s"', name)

    def _execute_cb(self, goal):

        # TODO: refactor this somewhere
        choices = {}
        for choice in goal.choices:
            if choice.id in choices:
                rospy.logwarn('duplicate key "%s" in answer', choice.id)
            else:
                choices[choice.id] = choice.values

        rospy.loginfo('I got a question: %s', goal.description)
        rospy.loginfo('This is the spec: %s, %s', trim_string(goal.spec), repr(choices))

        try:
            result = self._determine_answer(description=goal.description,
                                           spec=goal.spec,
                                           choices=choices,
                                           is_preempt_requested=self._server.is_preempt_requested)
        except Exception as e:
            # rospy.logwarn('_determine_answer raised an exception: %s', e)
            # import pdb; pdb.set_trace()
            tb = traceback.format_exc()
            rospy.logerr('_determine_answer raised an exception: %s' % tb)

            self._server.set_aborted()
        else:
            # we've got a result or a cancel
            if result:
                self._set_succeeded(result=result.to_ros(self._action_name))
                rospy.loginfo('result: %s', result)
            else:
                rospy.loginfo('cancelled')
                self._server.set_aborted(text="Cancelled by user")

    def _set_succeeded(self, result):
        self._server.set_succeeded(result)

    def _publish_feedback(self):
        self._server.publish_feedback(QueryActionFeedback())

    @abstractmethod
    def _determine_answer(self, description, spec, choices, is_preempt_requested):
        '''
        Overwrite this method to provide custom implementations

        Return the answer
        Return None if nothing is heared
        Raise an Exception if an error occured
        '''
        pass

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]
class HokuyoLaserActionServer():
    def __init__(self):
        # Initialize constants
        self.error_threshold = 0.0175 # Report success if error reaches below threshold
        self.signal = 1
        
        # Initialize new node
        rospy.init_node(NAME, anonymous=True)
        
        controller_name = rospy.get_param('~controller')
        
        # Initialize publisher & subscriber for tilt
        self.laser_tilt = JointControllerState()
        self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
        self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
        self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
        
        rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
        rospy.wait_for_message(controller_name + '/state', JointControllerState)
        
        # Initialize tilt action server
        self.result = HokuyoLaserTiltResult()
        self.feedback = HokuyoLaserTiltFeedback()
        self.feedback.tilt_position = self.laser_tilt.process_value
        self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
        
        rospy.loginfo("%s: Ready to accept goals", NAME)

    def process_tilt_state(self, tilt_data):
        self.laser_tilt = tilt_data

    def reset_tilt_position(self, offset=0.0):
        self.laser_tilt_pub.publish(offset)
        rospy.sleep(0.5)

    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.joint_speed_srv(2.0)
        self.reset_tilt_position(goal.offset)
        delta = goal.amplitude - goal.offset
        target_speed = delta / goal.duration
        timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration)
        self.joint_speed_srv(target_speed)
        
        print "delta = %f, target_speed = %f" % (delta, target_speed)
        
        self.result.success = True
        self.result.tilt_position = self.laser_tilt.process_value
        rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles)
        
        # Tilt laser goal.tilt_cycles amount of times.
        for i in range(1, goal.tilt_cycles + 1):
            self.feedback.progress = i
            
            # Issue 2 commands for each cycle
            for j in range(2):
                if j % 2 == 0:
                    target_tilt = goal.offset + goal.amplitude     # Upper tilt limit
                    self.signal = 0
                else:
                    target_tilt = goal.offset     # Lower tilt limit
                    self.signal = 1
                    
                # Publish target command to controller
                self.laser_tilt_pub.publish(target_tilt)
                start_time = rospy.Time.now()
                current_time = start_time
                
                while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold:
                    #delta = abs(target_tilt - self.laser_tilt.process_value)
                    #time_left = goal.duration - (rospy.Time.now() - start_time).to_sec()
                    #target_speed = delta / time_left
                    #self.joint_speed_srv(target_speed)
                    
                    # Cancel exe if another goal was received (i.e. preempt requested)
                    if self.server.is_preempt_requested():
                        rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                        self.result.success = False
                        self.server.set_preempted()
                        return
                        
                    # Publish current head position as feedback
                    self.feedback.tilt_position = self.laser_tilt.process_value
                    self.server.publish_feedback(self.feedback)
                    
                    # Abort if timeout
                    current_time = rospy.Time.now()
                    
                    if (current_time - start_time > timeout_threshold):
                        rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                        self.result.success = False
                        self.server.set_aborted()
                        return
                        
                    r.sleep()
                    
                signal = LaserScannerSignal()
                signal.header.stamp = current_time
                signal.signal = self.signal
                self.laser_signal_pub.publish(signal)
#.........这里部分代码省略.........

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]

#.........这里部分代码省略.........
        self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
        self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
        rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
        rospy.wait_for_message('finger_right_controller/state', JointControllerState)

        # Initialize action server
        self.result = SmartArmGripperResult()
        self.feedback = SmartArmGripperFeedback()
        self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
        self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)

        # Reset gripper position
        rospy.sleep(1)
        self.reset_gripper_position()
        rospy.loginfo("%s: Ready to accept goals", NAME)


    def reset_gripper_position(self):
        self.left_finger_pub.publish(0.0)
        self.right_finger_pub.publish(0.0)
        rospy.sleep(5)


    def read_left_finger(self, data):
        self.left_finger = data
        self.has_latest_left_finger = True


    def read_right_finger(self, data):
        self.right_finger = data
        self.has_latest_right_finger = True


    def wait_for_latest_controller_states(self, timeout):
        self.has_latest_left_finger = False
        self.has_latest_right_finger = False
        r = rospy.Rate(100)
        start = rospy.Time.now()
        while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \
                (rospy.Time.now() - start < timeout):
            r.sleep()


    def execute_callback(self, goal):
        r = rospy.Rate(100)
        self.result.success = True
        self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
        rospy.loginfo("%s: Executing move gripper", NAME)
        
        # Initialize target joints
        target_joints = list()
        for i in range(self.JOINTS_COUNT):
            target_joints.append(0.0)

        # Retrieve target joints from goal
        if (len(goal.target_joints) > 0):
            for i in range(min(len(goal.target_joints), len(target_joints))):
                target_joints[i] = goal.target_joints[i] 
        else:
            rospy.loginfo("%s: Aborted: Invalid Goal", NAME)
            self.result.success = False
            self.server.set_aborted()
            return

        # Publish goal to controllers
        self.left_finger_pub.publish(target_joints[0])
        self.right_finger_pub.publish(target_joints[1])

        # Initialize loop variables
        start_time = rospy.Time.now()

        while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \
                math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD):
		
	        # Cancel exe if another goal was received (i.e. preempt requested)
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.result.success = False
                self.server.set_preempted()
                break

            # Publish current gripper position as feedback
            self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
            self.server.publish_feedback(self.feedback)
            
            # Abort if timeout
            current_time = rospy.Time.now()
            if (current_time - start_time > self.TIMEOUT_THRESHOLD):
                rospy.loginfo("%s: Aborted: Action Timeout", NAME)
                self.result.success = False
                self.server.set_aborted()
                break

            r.sleep()

        if (self.result.success):
            rospy.loginfo("%s: Goal Completed", NAME)
            self.wait_for_latest_controller_states(rospy.Duration(2.0))
            self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
            self.server.set_succeeded(self.result)

# 需要导入模块: from actionlib import SimpleActionServer [as 别名]
# 或者: from actionlib.SimpleActionServer import publish_feedback [as 别名]
class ErraticBaseActionServer():
    def __init__(self):
        self.base_frame = '/base_footprint'
        
        self.move_client = SimpleActionClient('move_base', MoveBaseAction)
        self.move_client.wait_for_server()
        
        self.tf = tf.TransformListener()
        
        self.result = ErraticBaseResult()
        self.feedback = ErraticBaseFeedback()
        self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
        self.server.start()
        
        rospy.loginfo("%s: Ready to accept goals", NAME)


    def transform_target_point(self, point):
        self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
        return self.tf.transformPoint(self.base_frame, point)


    def move_to(self, target_pose):
        goal = MoveBaseGoal()
        goal.target_pose = target_pose
        goal.target_pose.header.stamp = rospy.Time.now()
        
        self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb)
        
        while not self.move_client.wait_for_result(rospy.Duration(0.01)):
            # check for preemption
            if self.server.is_preempt_requested():
                rospy.loginfo("%s: Aborted: Action Preempted", NAME)
                self.move_client.cancel_goal()
                return GoalStatus.PREEMPTED
                
        return self.move_client.get_state()


    def move_base_feedback_cb(self, fb):
        self.feedback.base_position = fb.base_position
        if self.server.is_active():
            self.server.publish_feedback(self.feedback)


    def get_vicinity_target(self, target_pose, vicinity_range):
        vicinity_pose = PoseStamped()
        
        # transform target to base_frame reference
        target_point = PointStamped()
        target_point.header.frame_id = target_pose.header.frame_id
        target_point.point = target_pose.pose.position
        self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0))
        target = self.tf.transformPoint(self.base_frame, target_point)
        
        rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z)
        
        # find distance to point
        dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
        
        if (dist < vicinity_range):
            # if already within range, then no need to move
            vicinity_pose.pose.position.x = 0.0
            vicinity_pose.pose.position.y = 0.0
        else:
            # normalize vector pointing from source to target
            target.point.x /= dist
            target.point.y /= dist
            
            # scale normal vector to within vicinity_range distance from target
            target.point.x *= (dist - vicinity_range)
            target.point.y *= (dist - vicinity_range)
            
            # add scaled vector to source
            vicinity_pose.pose.position.x = target.point.x + 0.0
            vicinity_pose.pose.position.y = target.point.y + 0.0
            
        # set orientation
        ori = Quaternion()
        yaw = math.atan2(target.point.y, target.point.x)
        (ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
        vicinity_pose.pose.orientation = ori
        
        # prep header
        vicinity_pose.header = target_pose.header
        vicinity_pose.header.frame_id = self.base_frame
        
        rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z)
        
        return vicinity_pose


    def execute_callback(self, goal):
        rospy.loginfo("%s: Executing move base", NAME)
        
        move_base_result = None
        
        if goal.vicinity_range == 0.0:
            # go to exactly
            move_base_result = self.move_to(goal.target_pose)
#.........这里部分代码省略.........