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)
