def execute(self, userdata): result = PickObjectWBCResult() result.success = False userdata.pick_object_wbc_result = result # give feedback feedback = PickObjectWBCFeedback() feedback.current_state = 'SELECT_OBJECT' userdata.pick_object_wbc_feedback = feedback # the goal will contain the arm location to move userdata.move_arm_to = userdata.pick_object_wbc_goal.object # do not kill the node so fast, let the topic to survive for some time rospy.loginfo('move arm to : ' + userdata.move_arm_to) return 'success'
def execute(self, userdata): # give feedback feedback = PickObjectWBCFeedback() feedback.current_state = 'SEND_EVENT' userdata.pick_object_wbc_feedback = feedback # creating string message msg = String() # filling message msg.data = self.event # publish self.publisher.publish(msg) rospy.loginfo('publishing on ' + self.topic_name + ' ' + self.event) # wait, dont kill the node so quickly rospy.sleep(0.2) return 'success'
def execute(self, userdata): # give feedback feedback = PickObjectWBCFeedback() feedback.current_state = 'WAIT_FOR_FEEDBACK' userdata.pick_object_wbc_feedback = feedback # reset flag of received to false print "waiting for node response..." self.message_received = False start_time = rospy.Time.now() rate = rospy.Rate(10) # 10hz # wait for message to arrive while((rospy.Time.now() - start_time < self.timeout) and not(self.message_received) and not(rospy.is_shutdown())): rate.sleep() # dont kill the node so quickly, wait for handshake of nodes print("(rospy.Time.now() - start_time < self.timeout) = {}".format((rospy.Time.now() - start_time < self.timeout))) print("self.message_received = {}".format((self.message_received))) rospy.sleep(0.2) return self.getResult()
def execute(self, userdata): result = PickObjectWBCResult() result.success = False userdata.pick_object_wbc_result = result # give feedback feedback = PickObjectWBCFeedback() feedback.current_state = 'SELECT_OBJECT' userdata.pick_object_wbc_feedback = feedback # receive parameters from actionlib object_to_pick = userdata.pick_object_wbc_goal.object.upper() # creating string message msg = String() # filling message msg.data = object_to_pick self.publisher.publish(msg) # do not kill the node so fast, let the topic to survive for some time rospy.loginfo('publishing on ' + self.topic_name + ' : ' + object_to_pick) rospy.sleep(0.2) return 'success'