def __init__(self): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('wbc_pick_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append(KeyValue(key='object', value='any'))
def __init__(self): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("wbc_pick_object_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append(KeyValue(key="object", value="any"))
def __init__(self): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient( "perceive_location_server", GenericExecuteAction ) self.client.wait_for_server() self.goal = GenericExecuteGoal()
def __init__(self, platform_name): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('place_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key='location', value=str(platform_name).upper()))
def __init__(self, perception_mode='three_view'): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("perceive_cavity_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key='perception_mode', value=perception_mode))
def __init__(self, destination_location): smach.State.__init__(self, outcomes=["success", "failed"], input_keys=["goal"]) self.client = SimpleActionClient("move_base_safe_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.destination_location = destination_location
def __init__(self, platform=None): smach.State.__init__(self, outcomes=["success", "failed"], input_keys=["goal"]) self.client = SimpleActionClient("unstage_object_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.platform = platform
def __init__(self, robot_platform, container): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("insert_object_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key="robot_platform", value=str(robot_platform).upper()) ) self.goal.parameters.append(KeyValue(key="hole", value=str(container).upper()))
def __init__(self, platform=None): smach.State.__init__(self, outcomes=["success", "failed"], input_keys=["goal"]) self.client = SimpleActionClient("unstage_object_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() if platform is not None: self.goal.parameters.append( KeyValue(key="platform", value=str(platform).upper()) )
def __init__(self, robot_platform, container): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('insert_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key='robot_platform', value=str(robot_platform).upper())) self.goal.parameters.append( KeyValue(key='hole', value=str(container).upper()))
def __init__(self, destination_location): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('move_base_safe_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key='arm_safe_position', value='barrier_tape')) self.goal.parameters.append( KeyValue(key='destination_location', value=str(destination_location).upper()))
def __init__(self, platform=None): smach.State.__init__(self, outcomes=['success', 'failed'], input_keys=['goal']) self.client = SimpleActionClient('unstage_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() if platform is not None: self.goal.parameters.append( KeyValue(key='platform', value=str(platform).upper()))
def __init__(self, destination_location): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("move_base_safe_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key="arm_safe_position", value="barrier_tape")) self.goal.parameters.append( KeyValue(key="destination_location", value=str(destination_location).upper()))
def __init__(self): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('perceive_cavity_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal()
import sys import rospy from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from diagnostic_msgs.msg import KeyValue from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal if __name__ == "__main__": rospy.init_node("perceive_cavity_client_tester") client = SimpleActionClient("perceive_cavity_server", GenericExecuteAction) client.wait_for_server() goal = GenericExecuteGoal() if len(sys.argv) > 1: location = str(sys.argv[1]).upper() else: location = "PP01" goal.parameters.append(KeyValue(key="location", value=location)) rospy.loginfo("Sending following goal to perceive cavity server") rospy.loginfo(goal) client.send_goal(goal) timeout = 15.0 finished_within_time = client.wait_for_result( rospy.Duration.from_sec(int(timeout))) if not finished_within_time:
def execute_navigation_task(self): client = actionlib.SimpleActionClient( "move_base_safe_server", GenericExecuteAction ) rospy.loginfo("Waiting for move_base_safe's actionlib server") client.wait_for_server() rospy.loginfo("Got move_base_safe's actionlib server sending goals") goal = GenericExecuteGoal() goal.parameters.append(KeyValue(key="arm_safe_position", value="barrier_tape")) # start logging and tell the refbox that we're logging self.pub_logger.publish("e_start") logging_status = atwork_ros_msgs.msg.LoggingStatus() logging_status.is_logging.data = True self.pub_logging_status.publish(logging_status) for task in self._tasks.tasks: # check if the task is navigation or transportation if atwork_ros_msgs.msg.Task.NAVIGATION == task.type.data: print self.location_type[task.navigation_task.location.type.data] + str( task.navigation_task.location.instance_id.data ).zfill(2) try: destination_location = self.location_type[ task.navigation_task.location.type.data ] + str(task.navigation_task.location.instance_id.data).zfill(2) goal.parameters.append( KeyValue(key="destination_location", value=destination_location) ) destination_orientation = self.orientation[ task.navigation_task.orientation.data ] goal.parameters.append( KeyValue( key="destination_orientation", value=destination_orientation ) ) timeout = 60.0 rospy.loginfo( "Sending action lib goal to move_base_safe_server" + " , destination : " + destination_location + " , orientation : " + destination_orientation ) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(timeout))) result = client.get_result() state = client.get_state() if result: if state == GoalStatus.SUCCEEDED: # wait for required time at location d = rospy.Duration( task.navigation_task.wait_time.data.secs, 0 ) rospy.sleep(d) else: rospy.logerr("Navigation task failure. TODO to retry") else: client.cancel_goal() rospy.logerr("No Result from client") except: rospy.logerr( "FAILED . Action server MOVE_BASE_SAFE to desitnaton %s ", destination_location, ) # Exiting when the action is done move to EXIT goal = GenericExecuteGoal() goal.parameters.append(KeyValue(key="arm_safe_position", value="barrier_tape")) goal.parameters.append(KeyValue(key="destination_location", value="EXIT")) timeout = 60.0 client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(timeout))) client.cancel_goal() rospy.loginfo("Reached EXIT ") # stop logging and tell the refbox that we've stopped logging self.pub_logger.publish("e_stop") logging_status = atwork_ros_msgs.msg.LoggingStatus() logging_status.is_logging.data = False self.pub_logging_status.publish(logging_status)