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'