def move_base_safe(arm_safe_position,
current_robot_location,
destination,
duration=150.0):
"""
example:
arm_safe_position : "folded"
current_robot_location : "START"
destination : "S1"
"""
print('move_base_safe ' + current_robot_location + ' ' + destination)
# mockup begins----
if mockup:
mockup_result = action_outcome_mockup('move_base_safe')
if mockup_result:
return True
else:
return replan()
else:
client = actionlib.SimpleActionClient('move_base_safe_server',
MoveBaseSafeAction)
client.wait_for_server()
goal = MoveBaseSafeGoal()
goal.arm_safe_position = arm_safe_position
goal.source_location = current_robot_location
goal.destination_location = destination
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 update_kb_perceive(location, success=True):
if success:
params = [['l', location]]
rosplan_update_knowledge(1,
'n/a',
'n/a',
'perceived',
params,
update_type='ADD_KNOWLEDGE')
rosplan_update_knowledge(1,
'n/a',
'n/a',
'perceived',
params,
update_type='REMOVE_GOAL')
else:
wiggle_base_pub.publish('e_trigger')
client = actionlib.SimpleActionClient('move_base_safe_server',
MoveBaseSafeAction)
client.wait_for_server()
goal = MoveBaseSafeGoal()
goal.arm_safe_position = 'folded'
try:
goal.source_location = "dummy_source"
goal.destination_location = location
timeout = 15.0
rospy.loginfo(
'Sending action lib goal to move_base_safe_server, source : ' +
goal.source_location + ' , destination : ' +
goal.destination_location)
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
print client.get_result()
except:
pass
def move_base(self, destination):
goal = MoveBaseSafeGoal()
goal.arm_safe_position = 'barrier_tape'
goal.source_location = ''
goal.destination_location = destination
timeout = 60.0
rospy.loginfo(
'Sending action lib goal to move_base_safe_server, source : ' +
goal.source_location + ' , destination : ' +
goal.destination_location)
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result(
rospy.Duration.from_sec(int(timeout)))
# cancel the goal
self.move_base_client.cancel_goal()
rospy.loginfo('Move base complete')
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 __init__(self, destination_location):
smach.State.__init__(self, outcomes=['success', 'failed'])
self.move_base_client = actionlib.SimpleActionClient(
'move_base_safe_server', MoveBaseSafeAction)
self.move_base_client.wait_for_server()
self.goal = MoveBaseSafeGoal()
self.goal.arm_safe_position = 'barrier_tape'
self.goal.source_location = ''
self.goal.destination_location = destination_location
def execute_navigation_task(self):
client = actionlib.SimpleActionClient('move_base_safe_server', MoveBaseSafeAction)
rospy.loginfo("Waiting for Movebase actionlib server")
client.wait_for_server()
rospy.loginfo("Got Movebase actionlib server sending goals")
goal = MoveBaseSafeGoal()
goal.arm_safe_position = 'folded'
# 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:
goal.source_location = ""
goal.destination_location = self.location_type[task.navigation_task.location.type.data] + \
str(task.navigation_task.location.instance_id.data ).zfill(2)
goal.destination_orientation = self.orientation[task.navigation_task.orientation.data]
timeout = 60.0
rospy.loginfo('Sending action lib goal to move_base_safe_server'
+ ' , destination : ' + goal.destination_location
+ ' , orientation : '+ goal.destination_orientation)
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
result = client.get_result()
if(result):
if True == result.success:
# 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 ", goal.destination_location)
#Exiting when the action is done move to EXIT
goal.destination_location = "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)
def execute(self, userdata):
result = PickObjectResult()
result.success = False
userdata.pick_object_result = resultresult = PickObjectResult()
result.success = False
userdata.pick_object_result = result
# give feedback
feedback = PickObjectFeedback()
feedback.current_state = 'MOVE_TO_CONVEYOR'
userdata.pick_object_feedback = feedback
goal = MoveBaseSafeGoal()
goal.source_location = 'anywhere'
goal.destination_location = self.destination_location
#rospy.loginfo('Sending actionlib goal to ' + self.action_server + ', destination: ',
# goal.destination_location + ' with timeout: ' + str(self.timeout))
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(self.timeout))
res = self.client.get_result()
if res and res.success:
return 'succeeded'
else:
return 'failed'
#! /usr/bin/env python
import rospy
import roslib
import actionlib
import sys
from mir_yb_action_msgs.msg import MoveBaseSafeAction, MoveBaseSafeGoal
if __name__ == '__main__':
rospy.init_node('move_base_safe_client_tester')
client = actionlib.SimpleActionClient('move_base_safe_server',
MoveBaseSafeAction)
client.wait_for_server()
goal = MoveBaseSafeGoal()
goal.arm_safe_position = 'barrier_tape'
if len(sys.argv) == 3: # 2 arguments was received : ok proceed
try:
goal.source_location = str(sys.argv[1])
goal.destination_location = str(sys.argv[2])
timeout = 60.0
rospy.loginfo(
'Sending action lib goal to move_base_safe_server, source : ' +
goal.source_location + ' , destination : ' +
goal.destination_location)
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
# cancel the goal
client.cancel_goal()
print client.get_result()
except:
def update_kb_pick_object(obj, location, gripper, success=True):
if success:
params = [['g', gripper], ['o', obj]]
rosplan_update_knowledge(1,
'n/a',
'n/a',
'holding',
params,
update_type='ADD_KNOWLEDGE')
rosplan_update_knowledge(1,
'n/a',
'n/a',
'holding',
params,
update_type='REMOVE_GOAL')
params = [['o', obj], ['l', location]]
rosplan_update_knowledge(1,
'n/a',
'n/a',
'on',
params,
update_type='REMOVE_KNOWLEDGE')
params = [['g', gripper]]
rosplan_update_knowledge(1,
'n/a',
'n/a',
'gripper_is_free',
params,
update_type='REMOVE_KNOWLEDGE')
#params = [['l', location]]
#rosplan_update_knowledge(1, 'n/a', 'n/a', 'perceived', params, update_type='REMOVE_KNOWLEDGE')
else:
#params = [['l', location]]
#rosplan_update_knowledge(1, 'n/a', 'n/a', 'perceived', params, update_type='REMOVE_KNOWLEDGE')
params = [['l', location]]
rosplan_update_knowledge(1,
'n/a',
'n/a',
'perceived',
params,
update_type='REMOVE_KNOWLEDGE')
client = actionlib.SimpleActionClient('move_base_safe_server',
MoveBaseSafeAction)
client.wait_for_server()
goal = MoveBaseSafeGoal()
goal.arm_safe_position = 'folded'
try:
goal.source_location = "dummy_source"
goal.destination_location = location
timeout = 15.0
rospy.loginfo(
'Sending action lib goal to after pick action failure move_base_safe_server, source : '
+ goal.source_location + ' , destination : ' +
goal.destination_location)
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(int(timeout)))
print client.get_result()
#wiggle_base_pub.publish('e_trigger')
except:
rospy.logwarn(
"Exception: while sending move base goal after pick action failure"
)