def perceive_location(location, obj, duration=150.0):
"""
location
"""
print('perceive_location ' + location)
# mockup begins----
if mockup:
mockup_result = action_outcome_mockup('perceive_location')
if mockup_result:
return True
else:
return replan()
else:
if obj == 'pp01_cavity':
client = actionlib.SimpleActionClient('perceive_cavity_server',
PerceiveLocationAction)
else:
client = actionlib.SimpleActionClient('perceive_location_server',
PerceiveLocationAction)
client.wait_for_server()
goal = PerceiveLocationGoal()
goal.location = location
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(duration))
rospy.loginfo('action server finished with the following response : ' +
str(client.get_result().success))
if client.get_result().success == False:
return replan()
else:
return True
def perceive_cavity(self):
goal = PerceiveLocationGoal()
goal.location = ""
self.cavity_perceive_client.send_goal(goal)
self.cavity_perceive_client.wait_for_result(
rospy.Duration.from_sec(15.0))
rospy.loginfo('Result from perceive cavity server ' +
str(self.cavity_perceive_client.get_result()))
def __init__(self):
smach.State.__init__(self, outcomes=['success', 'failed'])
self.perceive_location_client = actionlib.SimpleActionClient(
'perceive_location_server', PerceiveLocationAction)
self.perceive_location_client.wait_for_server()
self.goal = PerceiveLocationGoal()
self.goal.location = ""
def initialize_clients(self):
self.client_move = actionlib.SimpleActionClient(
'move_base_safe_server', MoveBaseSafeAction)
self.client_move.wait_for_server()
self.goal_move = MoveBaseSafeGoal()
self.client_perceive = actionlib.SimpleActionClient(
'perceive_location_server', PerceiveLocationAction)
self.client_perceive.wait_for_server()
self.goal_perceive = PerceiveLocationGoal()
self.client_pick = actionlib.SimpleActionClient(
'pick_object_server', PickObjectAction)
self.client_pick.wait_for_server()
self.goal_pick = PickObjectGoal()
self.client_stage = actionlib.SimpleActionClient(
'stage_object_server', StageObjectAction)
self.client_stage.wait_for_server()
self.goal_stage = StageObjectGoal()
self.client_unstage = actionlib.SimpleActionClient(
'unstage_object_server', UnStageObjectAction)
self.client_unstage.wait_for_server()
self.goal_unstage = UnStageObjectGoal()
self.client_place = actionlib.SimpleActionClient(
'place_object_server', PlaceObjectAction)
self.client_place.wait_for_server()
self.goal_place = PlaceObjectGoal()
def perceive(self, for_test='BMT'):
goal = PerceiveLocationGoal()
goal.location = ""
self.perceive_location_client.send_goal(goal)
self.perceive_location_client.wait_for_result(
rospy.Duration.from_sec(15.0))
self.perceive_location_client.cancel_goal()
received_flag = False
while (not received_flag):
result = self.perceive_location_client.get_result()
if (result):
received_flag = True
rospy.sleep(0.1)
self.perceived_objects = result.object_list
rospy.loginfo('Perceived objects : ' +
','.join(self.perceived_objects))
self.perceived_objects = self.prioritize_object_list(for_test)
rospy.loginfo('Priority Perceived objects : ' +
','.join(self.perceived_objects))
#! /usr/bin/env python
import rospy
import roslib
import actionlib
from mir_yb_action_msgs.msg import PerceiveLocationAction, PerceiveLocationGoal
if __name__ == '__main__':
rospy.init_node('perceive_cavity_client_tester')
client = actionlib.SimpleActionClient('perceive_cavity_server',
PerceiveLocationAction)
client.wait_for_server()
goal = PerceiveLocationGoal()
goal.location = "sh-03"
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(15.0))
print client.get_result()