class SceneGeometryActionClient(EventState):
'''
Actionlib_comm_state is an actionlib state that will make a call to an action server.
This is intended to serve as a template for future states that need to communicate with
action server. Currently this is set up to communicate with the data collection action
server and doesnt do anything other than return true or false.
TODO: Proper error checking
<= continue All actions completed, data collection started
<= failed Data control failed to initialize or call something TODO: Proper error checking
'''e
def __init__(self, instruction):
# See example_state.py for basic explanations.
super(SceneGeometryActionClient, self).__init__(outcomes = ['completed', 'failed'])
self._topic = 'scene_constraint_as'
self._client = ProxyActionClient({self._topic: SceneConstraintAction})
self._instruction = instruction
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# Check if the client failed to send the goal.
if self._error:
return 'failed'
#otherwise get the result and perform next steps
if(self._client.has_result(self._topic)):
result = self._client.get_result(self._topic)
status = result.constraint_result
if(status == True):
print("returned True")
return 'completed'
else:
print("returned False")
return 'failed'
def on_enter(self, userdata):
#Creating the goal to send
goal = SceneConstraintGoal()
goal.constraint_request = self._instruction
#error checking in case communication cant be established
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the start data collection command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Makes sure that the action is not running when leaving this state.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class StageActionClient(EventState):
'''
stage_ac_state is an action client state that will make a call to an action server.
This is intended to serve as a template for future states that need to communicate with
action server. The topic given should correspond to an action server and it right now only
works for StageAction action messages but in the future it will have any custom messages needed
once they are more finalized. Currently just sends true or false to an action server and
either returns complete or failed based on the response.
TODO: Proper error checking other custom messages
<= continue All actions completed, data collection started
<= failed Data control failed to initialize or call something TODO: Proper error checking
'''
def __init__(self, topic):
# See example_state.py for basic explanations.
super(StageActionClient,
self).__init__(outcomes=['completed', 'failed'])
self._topic = topic
self._client = ProxyActionClient({self._topic: StageAction})
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# Check if the client failed to send the goal.
if self._error:
return 'failed'
#otherwise get the result and perform next steps
if (self._client.has_result(self._topic)):
result = self._client.get_result(self._topic)
status = result.result
if (status == 0):
print("Completed Succesfully")
return 'completed'
else:
print("Error Thrown")
return 'failed'
def on_enter(self, userdata):
#Creating the goal to send
goal = StageGoal()
goal.start = 0
#error checking in case communication cant be established
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn(
'Failed to send the start data collection command:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Makes sure that the action is not running when leaving this state.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class TestRendezvousState(EventState):
'''
This is a test actionlib state used for the rendezvous behavior
-- goal_distance double The distance
># start Determin if the state should start from the beginning r if it needs to wait.
<= continue If a result is recieved from the actionlib server
<= failed Not really used at this point
'''
def __init__(self, goal_distance):
# Declaring outcomes, input keys by calling the super constructor with the corresponding arguments.
super(TestRendezvousState,
self).__init__(outcomes=['continue', 'failed'])
# Pass the topic the actionlib server is listening on
self._topic = 'test_rendezvous'
self._client = ProxyActionClient({self._topic: rendezvousAction})
self._error = False
def execute(self, userdata):
# Check if the client failed to send the goal.
if self._error:
return 'failed'
# Check if the action has been completed
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
Logger.loginfo('Rendezvous has talked')
return 'continue'
# If the action has not yet finished, no outcome will ne returned and will stay active.
def on_enter(self, userdata):
# When entering this state, we send the actoionlin server the goal once and then sit back and hope for the best
# Create the goal msg.
goal = rendezvousGoal()
goal.goal_dist = 1
# Send the goal msg
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the goal msg to server:\n%s' %
str(e))
def on_exit(self, userdata):
# Make sure that the action is not running when leabing this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Loger.loginfo('Cancelled active action goal.')
class MetricSweepState(EventState):
'''
Robot performs a metric sweep at its current location.
-- sweep_type string Type of the sweep to do (select one of the provided options).
<= sweeped Sweep has been completed.
<= failed There has been an error when sweeping.
'''
COMPLETE = 'complete'
MEDIUM = 'medium'
SHORT = 'short'
SHORTEST = 'shortest'
def __init__(self, sweep_type):
super(MetricSweepState, self).__init__(outcomes = ['sweeped', 'failed'])
self._sweep_type = sweep_type
self._topic = '/do_sweep'
self._client = ProxyActionClient({self._topic: SweepAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
if result.success:
return 'sweeped'
else:
return 'failed'
def on_enter(self, userdata):
goal = SweepGoal()
goal.type = self._sweep_type
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class MoveCameraState(EventState):
'''
Moves the camera.
># pan float Pan angle [-180, 180)
># tilt float Tilt angle [-41, 41]
<= done Moved the camera.
<= failed Cannot send the action goal.
'''
def __init__(self):
# See example_state.py for basic explanations.
super(MoveCameraState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['pan', 'tilt'])
self._topic = 'SetPTUState'
self._client = ProxyActionClient({self._topic: PtuGotoAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
return 'done'
def on_enter(self, userdata):
goal = PtuGotoGoal()
goal.pan = userdata.pan
goal.tilt = userdata.tilt
goal.pan_vel = 60
goal.tilt_vel = 60
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the camera movement command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class MetricSweepState(EventState):
'''
Robot performs a metric sweep at its current location.
-- sweep_type string Type of the sweep to do (select one of the provided options).
<= sweeped Sweep has been completed.
<= failed There has been an error when sweeping.
'''
COMPLETE = 'complete'
MEDIUM = 'medium'
SHORT = 'short'
SHORTEST = 'shortest'
def __init__(self, sweep_type):
super(MetricSweepState, self).__init__(outcomes=['sweeped', 'failed'])
self._sweep_type = sweep_type
self._topic = '/do_sweep'
self._client = ProxyActionClient({self._topic: SweepAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
if result.success:
return 'sweeped'
else:
return 'failed'
def on_enter(self, userdata):
goal = SweepGoal()
goal.type = self._sweep_type
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class SpeechOutputEnglishParamTextState(EventState):
'''
Lets the robot speak the string, set in Behaviour Parameters
variable or inplace in the state GUI.
-- text_to_speak string The text to be spoken.
<= done Speaking has finished.
<= failed Failed to execute speaking.
'''
def __init__(self, text_to_speak="Hello World!"):
super(SpeechOutputEnglishParamTextState,
self).__init__(outcomes=['done', 'failed'])
self._topic = '/sound_play'
self._text_to_speak = text_to_speak
self._client = ProxyActionClient({self._topic: SoundRequestAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
return 'done'
def on_enter(self, userdata):
Logger.loginfo('Speech output, talking')
goal = SoundRequestGoal()
goal.sound_request.arg = self._text_to_speak
goal.sound_request.command = SoundRequest.PLAY_ONCE
goal.sound_request.sound = SoundRequest.SAY
goal.sound_request.volume = 1.0
self._error = False
try:
self._client.send_goal(self._topic, goal)
# Logger.logwarn('V try')
except Exception as e:
Logger.logwarn('Failed to send the Speech command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class SpeechOutputState(EventState):
'''
Lets the robot speak the given input string.
># text string Text to output.
<= done Speaking has finished.
<= failed Failed to execute speaking.
'''
def __init__(self):
super(SpeechOutputState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['text'])
self._topic = '/speak'
self._client = ProxyActionClient({self._topic: maryttsAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
return 'done'
def on_enter(self, userdata):
goal = maryttsGoal()
goal.text = userdata.text
print 'speech_output_state: say : ', goal.text
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class SpeechOutputState(EventState):
'''
Lets the robot speak the given input string.
># text string Text to output.
<= done Speaking has finished.
<= failed Failed to execute speaking.
'''
def __init__(self):
super(SpeechOutputState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['text'])
self._topic = '/speak'
self._client = ProxyActionClient({self._topic: maryttsAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
return 'done'
def on_enter(self, userdata):
goal = maryttsGoal()
goal.text = userdata.text
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class SpeechOutputViaSoundPlayState(EventState):
'''
Lets the robot speak the given input string.
># text_IN string Text to output.
<= done Speaking has finished.
<= failed Failed to execute speaking.
'''
def __init__(self):
super(SpeechOutputViaSoundPlayState,
self).__init__(outcomes=['done', 'failed'],
input_keys=['text_IN'])
self._topic = '/sound_play'
self._client = ProxyActionClient({self._topic: SoundRequestAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
return 'done'
def on_enter(self, userdata):
Logger.loginfo('Speech output, talking')
goal = SoundRequestGoal()
goal.sound_request.arg = userdata.text_IN
goal.sound_request.command = SoundRequest.PLAY_ONCE
goal.sound_request.sound = SoundRequest.SAY
goal.sound_request.volume = 1.0
self._error = False
try:
self._client.send_goal(self._topic, goal)
# Logger.logwarn('V try')
except Exception as e:
Logger.logwarn('Failed to send the Speech command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class NavigateState(EventState):
'''
Navigate to waypoints.
># target_pose Dict[str,float] Pose to navigate to, a dictionary with the keys 'x', 'y', and 'frame_id'
<= ok Navigation complete with no errors
<= err Navigation errors were encountered
'''
def __init__(self):
super(NavigateState, self).__init__(outcomes=['ok', 'err'],
input_keys=['target_pose'])
self._topic = 'move_base'
self._client = ProxyActionClient({self._topic: MoveBaseAction})
self._error = False
def execute(self, ud):
if self._error:
return 'err'
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic) # type: GoalStatus
return 'ok' if status == GoalStatus.SUCCEEDED else 'err'
def on_enter(self, userdata):
goal = MoveBaseGoal()
goal.target_pose = PoseStamped()
goal.target_pose.header.frame_id = userdata.target_pose['frame_id']
goal.target_pose.pose.position.x = userdata.target_pose['x']
goal.target_pose.pose.position.y = userdata.target_pose['y']
goal.target_pose.pose.orientation.w = 1.0
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as err:
Logger.logwarn(
'Could not send nav goal to move_base:\n{}'.format(err))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
class SpeechOutputState(EventState):
"""
Lets the robot speak the given input string.
># text string Text to output.
<= done Speaking has finished.
<= failed Failed to execute speaking.
"""
def __init__(self):
super(SpeechOutputState, self).__init__(outcomes=["done", "failed"], input_keys=["text"])
self._topic = "/speak"
self._client = ProxyActionClient({self._topic: maryttsAction})
self._error = False
def execute(self, userdata):
if self._error:
return "failed"
if self._client.has_result(self._topic):
return "done"
def on_enter(self, userdata):
goal = maryttsGoal()
goal.text = userdata.text
Logger.loginfo("Speech output, saying:\n%s" % goal.text)
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn("Failed to send the Sweep command:\n%s" % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo("Cancelled active action goal.")
def test_action_client(self):
t1 = '/action_1'
server = None
def execute_cb(goal):
rospy.sleep(.5)
if server.is_preempt_requested():
server.set_preempted()
else:
server.set_succeeded(BehaviorExecutionResult(outcome='ok'))
server = actionlib.SimpleActionServer(t1,
BehaviorExecutionAction,
execute_cb,
auto_start=False)
server.start()
client = ProxyActionClient({t1: BehaviorExecutionAction})
self.assertFalse(client.has_result(t1))
client.send_goal(t1, BehaviorExecutionGoal())
rate = rospy.Rate(20)
for i in range(20):
self.assertTrue(client.is_active(t1) or client.has_result(t1))
rate.sleep()
self.assertTrue(client.has_result(t1))
result = client.get_result(t1)
self.assertEqual(result.outcome, 'ok')
client.send_goal(t1, BehaviorExecutionGoal())
rospy.sleep(.1)
client.cancel(t1)
rospy.sleep(.9)
self.assertFalse(client.is_active(t1))
self.assertFalse(client.is_available('/not_there'))
client = ProxyActionClient({'/invalid': BehaviorExecutionAction},
wait_duration=.1)
self.assertFalse(client.is_available('/invalid'))
class BrickGraspingActionState(EventState):
""" brick grasping results """
RESULT_SUCCESS = 0
RESULT_STOPPED_BY_COMMAND = 1
RESULT_OBJECT_NOT_RELIABLE = 2
RESULT_OBJECT_NOT_VISIBLE = 3
RESULT_GRASPING_TIMOUT = 4
RESULT_GRASPING_FAILED = 5
""" brick grasping object types """
OBJECT_RED = 1
OBJECT_GREEN = 2
OBJECT_BLUE = 3
OBJECT_ORANGE = 4
WALL = 5
""" result ids """
RESULT_SUCCESS = 0
RESULT_STOPPED_BY_COMMAND = 1
RESULT_OBJECT_NOT_RELIABLE = 2
RESULT_OBJECT_NOT_VISIBLE = 3
RESULT_GRASPING_TIMOUT = 4
RESULT_GRASPING_FAILED = 5
def __init__(self, return_altitude):
# See example_state.py for basic explanations.
super(BrickGraspingActionState,
self).__init__(outcomes=['grasped', 'grasping_error'],
input_keys=['brick_type'])
self._topic = 'brick_grasping'
self._client = ProxyActionClient(
{self._topic:
GraspingAction}) # pass required clients as dict (topic: type)
self._return_altitude = return_altitude
Logger.loginfo('[GraspingAction]: Client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'grasping_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
Logger.loginfo("[GraspingAction]: ended %s" % str(result.message))
# In this example, we also provide the amount of cleaned dishes as output key.
if result.success:
return 'grasped'
else:
return 'grasping_error'
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
rospy.loginfo_throttle(
5, "[GraspingAction]: Current feedback msg: %s" %
str(feedback.message))
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
brick_type = userdata.brick_type
# Create the goal.
goal = GraspingGoal()
#fill the goal
Logger.loginfo("[GraspingAction]: want to grasp brick type %s" %
(str(brick_type)))
goal.goal = brick_type
goal.return_altitude = self._return_altitude #where to return after grasping
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
Logger.loginfo("[GraspingAction]: sending goal %s to topic %s" %
(str(goal), str(self._topic)))
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'[GraspingAction]: Failed to send the GraspingAction command:\n%s'
% str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[GraspingAction]: Cancelled active action goal.')
class TweetPictureState(EventState):
'''
State for tweeting a picture and text
'''
def __init__(self):
# See example_state.py for basic explanations.
super(TweetPictureState, self).__init__(
outcomes=['picture_tweeted', 'tweeting_failed', 'command_error'],
input_keys=['picture_path', 'tweet_text'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'strands_tweets'
self._client = ProxyActionClient(
{self._topic:
SendTweetAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'command_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
if result.success:
return 'picture_tweeted'
else:
return 'tweeting_failed'
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
# Create the goal.
tweet = SendTweetGoal()
tweet.text = userdata.tweet_text
if len(userdata.tweet_text) > 140:
tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!'
tweet.with_photo = True
img = cv2.imread(userdata.picture_path)
bridge = CvBridge()
image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8")
tweet.photo = image_message
try:
self._client.send_goal(self._topic, tweet)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn(
'Failed to send the TweetPictureState command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class TestActionState(EventState):
'''
This is a test actionlib state to see how to actually make our own actionlib states that call an actionlib server using the state as a client. I will try to keep documentation but no promises.
This state takes doesn't take any input and just goes to finish once it recieves the goal from the server.
<= communicated_with_server Was able to connect and communicate with the actionlib server.
<= no_communication_with_server Was not able to connect and communicate with the actionlib server.
'''
def __init__(self):
# See example_state.py for basic explanations.
super(TestActionState, self).__init__(outcomes=[
'communicated_with_server', 'no_communication_with_server'
])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'test_action'
self._client = ProxyActionClient(
{self._topic:
testAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# Check if the client failed to send the goal.
if self._error:
return 'no_communication_with_server'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
Logger.loginfo('the result is %s' % result)
return 'communicated_with_server'
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = testGoal()
goal.test_goal = "test test"
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn('Failed to send the test_goal command:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class MoveBaseState(EventState):
"""
Navigates a robot to a desired position and orientation using move_base.
># waypoint Pose2D Target waypoint for navigation.
<= arrived Navigation to target pose succeeded.
<= failed Navigation to target pose failed.
"""
def __init__(self):
"""Constructor"""
super(MoveBaseState, self).__init__(outcomes=['arrived', 'failed'],
input_keys=['waypoint'])
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._arrived = False
self._failed = False
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
if self._arrived:
return 'arrived'
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
if status == GoalStatus.SUCCEEDED:
self._arrived = True
return 'arrived'
elif status in [
GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED
]:
Logger.logwarn('Navigation failed: %s' % str(status))
self._failed = True
return 'failed'
def on_enter(self, userdata):
"""Create and send action goal"""
self._arrived = False
self._failed = False
# Create and populate action goal
goal = MoveBaseGoal()
pt = Point(x=userdata.waypoint.x, y=userdata.waypoint.y)
qt = transformations.quaternion_from_euler(0, 0,
userdata.waypoint.theta)
goal.target_pose.pose = Pose(position=pt, orientation=Quaternion(*qt))
goal.target_pose.header.frame_id = "odom"
# goal.target_pose.header.stamp.secs = 5.0
# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" %
str(e))
self._failed = True
def cancel_active_goals(self):
if self._client.is_available(self._action_topic):
if self._client.is_active(self._action_topic):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo('Cancelled move_base active action goal.')
def on_exit(self, userdata):
self.cancel_active_goals()
def on_stop(self):
self.cancel_active_goals()
class SaraFollow(EventState):
"""
Make Sara follow an entity using move_base.
-- Distance float distance to keep between sara and the entity
-- ReplanPeriod float Time in seconds between each replanning for movebase
># ID int entity ID.
"""
def __init__(self, distance=1, ReplanPeriod=0.5):
"""Constructor"""
super(SaraFollow, self).__init__(outcomes=['failed'],
input_keys=['ID'])
# Prepare the action client for move_base
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
# Initialise the subscribers
self.entities_topic = "/entities"
self._pose_topic = "/robot_pose"
self._sub = ProxySubscriberCached({
self._pose_topic: Pose,
self.entities_topic: Entities
})
# Get the parameters
self.targetDistance = distance
self.ReplanPeriod = ReplanPeriod
# Initialise the status variables
self.MyPose = None
self.lastPlanTime = 0
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
# Initialise the Entity
Entity = None
# Get my last position
if self._sub.has_msg(self._pose_topic):
self.MyPose = self._sub.get_last_msg(self._pose_topic)
# Get the entity's position
if self._sub.has_msg(self.entities_topic):
message = self._sub.get_last_msg(self.entities_topic)
for entity in message.entities:
if entity.ID == userdata.ID:
Entity = entity
# If both my pose and the entyties are known, we start following.
if self.MyPose and Entity:
# Calculate a new goal if the desired period has passed.
if self.lastPlanTime + self.ReplanPeriod < rospy.get_time():
self.lastPlanTime = rospy.get_time()
self.setGoal(
self.generateGoal(self.MyPose.position, Entity.position))
# Check if movebase can reach the goal
if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
if status in [
GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED
]:
Logger.logwarn('Navigation failed: %s' % str(status))
return 'failed'
def on_enter(self, userdata):
"""Clean the costmap"""
rospy.wait_for_service('/move_base/clear_costmaps')
serv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
serv()
self.MyPose = None
self.Entity = None
self.Counter = 0
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo('Cancelled active action goal.')
def on_pause(self):
pose = self._client.get_feedback(self._action_topic)
self.setGoal(pose)
def generateGoal(self, MyPosition, EntityPosition):
# Calculate the safe distance to reach
GoalPose = Pose()
# Calculate the diference in position
dx = EntityPosition.x - MyPosition.x
dy = EntityPosition.y - MyPosition.y
# Calculate the desired distance to the person.
# If the person is too close, the robot must not go backward.
distanceToPerson = max(self.targetDistance, (dx**2 + dy**2)**0.5)
# Calculate the desired position to reach.
GoalPose.position.x = EntityPosition.x - dx / distanceToPerson * self.targetDistance
GoalPose.position.y = EntityPosition.y - dy / distanceToPerson * self.targetDistance
# Calculate the desired direction.
angle = math.atan2(dy, dx)
qt = quaternion_from_euler(0, 0, angle)
GoalPose.orientation.w = qt[3]
GoalPose.orientation.x = qt[0]
GoalPose.orientation.y = qt[1]
GoalPose.orientation.z = qt[2]
return GoalPose
def setGoal(self, pose):
# Generate the goal
goal = MoveBaseGoal()
goal.target_pose.pose = pose
goal.target_pose.header.frame_id = "map"
# Send the action goal for execution
try:
Logger.loginfo("sending goal" + str(goal))
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" %
str(e))
self._failed = True
class MoveToWaypointState(EventState):
'''
Lets the robot move to a given waypoint.
># waypoint PoseStamped Specifies the waypoint to which the robot should move.
># victim string object_id of detected object
># speed Speed of the robot
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
<= update Update the pose of current waypoint
'''
def __init__(self):
'''
Constructor
'''
super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed', 'update'],
input_keys=['waypoint','victim','speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance)
self._failed = False
self._reached = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
if self._move_client.has_result(self._action_topic):
result = self._move_client.get_result(self._action_topic)
if result.result == 1:
self._reached = True
return 'reached'
else:
self._failed = True
Logger.logwarn('Failed to reach waypoint!')
return 'failed'
self._currentTime = Time.now()
if self._currentTime.secs - self._startTime.secs >= 10:
return 'update'
def on_enter(self, userdata):
self._startTime = Time.now()
self._failed = False
self._reached = False
goal_id = GoalID()
goal_id.id = 'abcd'
goal_id.stamp = Time.now()
action_goal = MoveBaseGoal()
action_goal.target_pose = userdata.waypoint
action_goal.speed = userdata.speed
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
self._move_client.send_goal(self._action_topic, action_goal)
resp = self.set_tolerance(goal_id, 0.2, 1.55)
except Exception as e:
Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
Logger.loginfo('Driving to next waypoint')
def on_stop(self):
try:
if self._move_client.is_available(self._action_topic) \
and not self._move_client.has_result(self._action_topic):
self._move_client.cancel(self._action_topic)
except:
# client already closed
pass
def on_exit(self, userdata):
try:
if self._move_client.is_available(self._action_topic) \
and not self._move_client.has_result(self._action_topic):
self._move_client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._move_client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class SrdfStateToMoveit(EventState):
'''
State to look up a pre-defined joint configuration from the SRDF file loaded in the parameter server (/robot_description_semantic)
and send it to MoveIt to plan and move.
-- config_name string Name of the joint configuration of interest.
-- move_group string Name of the move group to be used for planning.
-- action_topic string Topic on which MoveIt is listening for action calls.
-- robot_name string Optional name of the robot to be used.
If left empty, the first one found will be used
(only required if multiple robots are specified in the same file).
># joint_values float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self,
config_name,
move_group="",
action_topic='/move_group',
robot_name=""):
'''
Constructor
'''
super(SrdfStateToMoveit,
self).__init__(outcomes=[
'reached', 'planning_failed', 'control_failed', 'param_error'
],
output_keys=[
'config_name', 'move_group', 'robot_name',
'action_topic', 'joint_values', 'joint_names'
])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerr(
'Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
def execute(self, userdata):
if self._param_error:
return 'param_error'
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn(
'Control failed for move action of group: %s (error code: %s)'
% (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn(
'Move action failed with result error code: %s' %
str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
#Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn(
'Unable to parse given SRDF parameter: /robot_description_semantic'
)
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib[
'name']:
robot = r
userdata.robot_name = robot # Save robot name to output key
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' %
self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib[
'group'] # Set move group name in case it was not defined
userdata.config_name = config # Save configuration name to output key
userdata.move_group = self._move_group # Save move_group to output key
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' %
self._config_name)
return 'param_error'
try:
self._joint_config = [
float(j.attrib['value']) for j in config.iter('joint')
]
self._joint_names = [
str(j.attrib['name']) for j in config.iter('joint')
]
userdata.joint_values = self._joint_config # Save joint configuration to output key
userdata.joint_names = self._joint_names # Save joint names to output key
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
str(e))
return 'param_error'
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class MotionServiceState(EventState):
"""Implements a state where a certain motion is performed.
This state can be used to execute motions created by the motion editor.
-- motion_key string Reference to the motion to be executed.
-- time_factor float Factor to multiply with the default execution time of the specified motion.
For example, 2 will result in a motion twice as long, thus executed at half speed.
<= done Indicates that the expected execution time of the motion has passed.
The motion service provides no way to check if a motion has actually finished.
<= failed Indicates that the requested motion doesn't exist and therefore wasn't executed
"""
def __init__(self, motion_key, time_factor=1):
"""
Constructor
"""
super(MotionServiceState, self).__init__(outcomes=['done', 'failed'])
self.motion_key = motion_key
self.time_factor = time_factor
self._finish_time = None
self._motion_goal_ns = '/motion_service/motion_goal'
self._client = ProxyActionClient({self._motion_goal_ns: ExecuteMotionAction})
self._failed = False
self._done = False
def execute(self, userdata):
"""
Execute this state
"""
if self._failed:
return 'failed'
if self._done:
return 'done'
if self._client.has_result(self._motion_goal_ns):
result = self._client.get_result(self._motion_goal_ns)
if result is None: # Bug in actionlib, sometimes returns None instead of result
# Operator decision needed
Logger.logwarn("Failed to execute the motion '%s':\nAction result is None" % self.motion_key)
self._failed = True
return 'failed'
if result.error_string is None or len(result.error_code) == 0:
Logger.logwarn("Failed to execute the motion '%s':\nAction result is None" % self.motion_key)
self._failed = True
return 'failed'
success = all(lambda x: x == 0, result.error_code)
if success:
rospy.loginfo('Trajectory finished successfully.') # dont need to send this to the operator
self._done = True
return 'done'
else:
Logger.logwarn("Failed to execute the motion '%s':\n%s" % (self.motion_key, str(result.error_code)))
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
self._done = False
# build goal
goal = ExecuteMotionGoal()
goal.motion_name = self.motion_key
goal.time_factor = self.time_factor
try:
self._client.send_goal(self._motion_goal_ns, goal)
except Exception as e:
Logger.logwarn("Failed sending motion goal for '%s':\n%s" % (self.motion_key, str(e)))
def on_exit(self, userdata):
if not self._client.has_result(self._motion_goal_ns):
self._client.cancel(self._motion_goal_ns)
Logger.loginfo("Cancelled active motion goal.")
class MoveBaseState(EventState):
"""
Navigates a robot to a desired position and orientation using move_base.
># waypoint Pose2D Target waypoint for navigation.
<= arrived Navigation to target pose succeeded.
<= failed Navigation to target pose failed.
"""
def __init__(self):
"""Constructor"""
super(MoveBaseState, self).__init__(outcomes = ['arrived', 'failed'],
input_keys = ['waypoint'])
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._arrived = False
self._failed = False
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
if self._arrived:
return 'arrived'
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
if status == GoalStatus.SUCCEEDED:
self._arrived = True
return 'arrived'
elif status in [GoalStatus.PREEMPTED, GoalStatus.REJECTED,
GoalStatus.RECALLED, GoalStatus.ABORTED]:
Logger.logwarn('Navigation failed: %s' % str(status))
self._failed = True
return 'failed'
def on_enter(self, userdata):
"""Create and send action goal"""
self._arrived = False
self._failed = False
# Create and populate action goal
goal = MoveBaseGoal()
pt = Point(x = userdata.waypoint.x, y = userdata.waypoint.y)
qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta)
goal.target_pose.pose = Pose(position = pt,
orientation = Quaternion(*qt))
goal.target_pose.header.frame_id = "odom"
# goal.target_pose.header.stamp.secs = 5.0
# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo('Cancelled active action goal.')
class ExampleActionState(EventState):
"""
Actionlib actions are the most common basis for state implementations
since they provide a non-blocking, high-level interface for robot capabilities.
The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib).
This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state.
-- dishes_to_do int Expected amount of dishes to be cleaned.
># dishwasher int ID of the dishwasher to be used.
#> cleaned int Amount of cleaned dishes.
<= cleaned_some Only a few dishes have been cleaned.
<= cleaned_enough Cleaned a lot of dishes.
<= command_error Cannot send the action goal.
"""
def __init__(self, dishes_to_do):
# See example_state.py for basic explanations.
super(ExampleActionState, self).__init__(
outcomes=["cleaned_some", "cleaned_enough", "command_error"],
input_keys=["dishwasher"],
output_keys=["cleaned"],
)
self._dishes_to_do = dishes_to_do
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = "do_dishes"
self._client = ProxyActionClient({self._topic: DoDishesAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return "command_error"
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
dishes_cleaned = result.total_dishes_cleaned
# In this example, we also provide the amount of cleaned dishes as output key.
userdata.cleaned = dishes_cleaned
# Based on the result, decide which outcome to trigger.
if dishes_cleaned > self._dishes_to_do:
return "cleaned_enough"
else:
return "cleaned_some"
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
dishwasher_id = userdata.dishwasher
# Create the goal.
goal = DoDishesGoal()
goal.dishwasher_id = dishwasher_id
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn("Failed to send the DoDishes command:\n%s" % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo("Cancelled active action goal.")
class GripperCommandState(EventState):
"""
Executes a custom trajectory.
-- gripper_cmd int Identifier of the pre-defined commands.
The state's class variables can be used here.
-- time float Relative time in seconds from starting
the execution to when the corresponding
target gripper state should be reached.
<= done Gripper command was successfully executed.
<= failed Failed to send or execute gripper command.
"""
CLOSE_GRIPPER = 0
OPEN_GRIPPER = 1
def __init__(self, gripper_cmd, time=5.0):
"""Constructor"""
super(GripperCommandState, self).__init__(outcomes=["done", "failed"])
self._joint_names = ["gripper_finger_joint_l", "gripper_finger_joint_r"]
self._gripper_joint_positions = {
# gripper_finger_joint_l, gripper_finger_joint_r
0: [0.001, 0.001],
1: [0.010, 0.010], # NOTE: 0.011 results in goal tolerance violation
}
self._gripper_cmd = gripper_cmd
self._time = time
self._action_topic = "/arm_1/gripper_controller/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._done = False
self._failed = False
def execute(self, userdata):
"""Wait for action result and return outcome accordingly"""
if self._done:
return "done"
if self._failed:
return "failed"
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
self._done = True
return "done"
elif result.error_code == FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED:
Logger.logwarn("Probably done, but goal tolerances violated (%d)" % result.error_code)
self._done = True
return "done"
else:
Logger.logwarn(
"Gripper trajectory failed to execute: (%d) %s" % (result.error_code, result.error_string)
)
self._failed = True
return "failed"
def on_enter(self, userdata):
"""Create and send action goal"""
self._done = False
self._failed = False
# Create and populate action goal
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = self._joint_names
target_joint_positions = self._gripper_joint_positions[self._gripper_cmd]
point = JointTrajectoryPoint()
point.positions = target_joint_positions
point.time_from_start = rospy.Duration.from_sec(self._time)
goal.trajectory.points.append(point)
# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send follow joint trajectory action goal:\n%s" % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class LookAtWaypoint(EventState):
"""
Lets the robot move to a given waypoint.
># waypoint PoseStamped Specifies the waypoint to which the robot should move.
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
"""
def __init__(self):
"""
Constructor
"""
super(LookAtWaypoint, self).__init__(outcomes=["reached", "failed"], input_keys=["waypoint"])
self._action_topic = "/pan_tilt_sensor_head_joint_control/look_at"
self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction})
self._failed = False
self._reached = False
def execute(self, userdata):
"""
Execute this state
"""
# if self._failed:
# return 'failed'
if self._reached:
return "reached"
# if self._move_client.has_result(self._action_topic):
# result = self._move_client.get_result(self._action_topic)
# if result.result == 1:
# self._reached = True
# return 'reached'
# else:
# self._failed = True
# Logger.logwarn('Failed to look at waypoint!')
# return 'failed'
def on_enter(self, userdata):
self._failed = False
self._reached = False
action_goal = hector_perception_msgs.msg.LookAtGoal()
action_goal.look_at_target.target_point.point = userdata.waypoint.pose.position
if action_goal.look_at_target.target_point.header.frame_id == "":
action_goal.look_at_target.target_point.header.frame_id = "map"
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn(
"Failed to send LookAt request to waypoint (%(x).3f, %(y).3f):\n%(err)s"
% {"err": str(e), "x": userdata.waypoint.pose.position.x, "y": userdata.waypoint.pose.position.y}
)
self._failed = True
Logger.loginfo("Looking at next waypoint")
self._reached = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class PipeDetectionState(EventState):
"""
Triggers pipe detection to find the location of the pipes
-- max_attempts int Maximum number of re-running attempts.
0 means unlimitied.
<= found Found the pipe location.
<= unknown Did not detect any pipes.
"""
def __init__(self, max_attempts=1):
"""
Constructor
"""
super(PipeDetectionState, self).__init__(outcomes=["found", "unknown"])
self._action_topic = "/hector_five_pipes_detection_node/detect"
self._client = ProxyActionClient({self._action_topic: DetectObjectAction})
self._max_attempts = max_attempts
self._success = False
self._failed = False
def execute(self, userdata):
"""
Execute this state
"""
if self._failed:
return "unknown"
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.detection_success:
return "found"
elif self._max_attempts == 0:
Logger.loginfo("Did not detect pipes, trying again (unlimited)...")
self.on_enter(userdata)
elif self._max_attempts > 1:
self._max_attempts -= 1
Logger.loginfo("Did not detect pipes, trying again (%d more times)..." % self._max_attempts)
self.on_enter(userdata)
else:
return "unknown"
def on_enter(self, userdata):
self._failed = False
action_goal = DetectObjectGoal()
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn("Failed to send pipe detection request:\n%s" % str(e))
self._failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class MoveToFixedWaypoint(EventState):
'''
Lets the robot move to a given waypoint.
-- allow_backwards Boolean Allow the robot to drive backwards when approaching the given waypoint.
># waypoint PoseStamped Specifies the waypoint to which the robot should move.
># speed Speed of the robot
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
'''
def __init__(self, allow_backwards = False):
'''
Constructor
'''
super(MoveToFixedWaypoint, self).__init__(outcomes=['reached', 'failed'],
input_keys=['waypoint','speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
#self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance)
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._allow_backwards = allow_backwards
self._failed = False
self._reached = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
if self._move_client.has_result(self._action_topic):
result = self._move_client.get_result(self._action_topic)
self._dynrec.update_configuration({'speed':self._defaultspeed})
if result.result == 1:
self._reached = True
return 'reached'
else:
self._failed = True
Logger.logwarn('Failed to reach waypoint!')
return 'failed'
def on_enter(self, userdata):
speedValue = self._dynrec.get_configuration(timeout = 0.5)
if speedValue is not None:
self._defaultspeed = speedValue['speed']
self._dynrec.update_configuration({'speed':userdata.speed})
self._startTime = Time.now()
self._failed = False
self._reached = False
#goal_id = GoalID()
#goal_id.id = 'abcd'
#goal_id.stamp = Time.now()
action_goal = MoveBaseGoal()
action_goal.target_pose = userdata.waypoint
action_goal.speed = userdata.speed
action_goal.reverse_allowed = self._allow_backwards
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
self._move_client.send_goal(self._action_topic, action_goal)
#resp = self.set_tolerance(goal_id, 0.2, 1.55)
except Exception as e:
Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
Logger.loginfo('Driving to next waypoint')
def on_stop(self):
try:
if self._move_client.is_available(self._action_topic) \
and not self._move_client.has_result(self._action_topic):
self._move_client.cancel(self._action_topic)
except:
# client already closed
pass
def on_exit(self, userdata):
try:
if self._move_client.is_available(self._action_topic) \
and not self._move_client.has_result(self._action_topic):
self._move_client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._move_client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class CreateStepGoalState(EventState):
'''
Creates a footstep goal from a desired pose.
-- pose_is_pelvis boolean Set this to True if the pose is given
as pelvis pose and not on the ground.
># target_pose PoseStamped Pose to which the robot should walk.
#> step_goal Feet Desired feet pose.
<= done Successfully created a step goal.
<= failed Failed to create a plan.
'''
def __init__(self, pose_is_pelvis=False):
'''Constructor'''
super(CreateStepGoalState, self).__init__(outcomes=['done', 'failed'],
input_keys=['target_pose'],
output_keys=['step_goal'])
self._action_topic = "/vigir/footstep_manager/generate_feet_pose"
self._client = ProxyActionClient(
{self._action_topic: GenerateFeetPoseAction})
self._pose_is_pelvis = pose_is_pelvis
self._done = False
self._failed = False
def execute(self, userdata):
'''Execute this state'''
if self._failed:
return 'failed'
if self._done:
return 'done'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.status.error == ErrorStatus.NO_ERROR:
userdata.step_goal = result.feet
self._done = True
return 'done'
else:
Logger.logwarn('Step goal creation failed:\n%s' %
result.status.error_msg)
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Upon entering the state, request the feet pose.'''
self._done = False
self._failed = False
# Create request msg and action goal
request = FeetPoseRequest()
request.header = userdata.target_pose.header
request.header.stamp = rospy.Time.now() # timestamp used to track goal
request.pose = userdata.target_pose.pose
request.flags = FeetPoseRequest.FLAG_PELVIS_FRAME if self._pose_is_pelvis else 0
action_goal = GenerateFeetPoseGoal(request=request)
# Send action goal
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send step goal request')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
'''Destructor'''
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class MoveArmState(EventState):
'''
Lets the robot move its arm.
># joint_config float[] Target joint configuration of the arm.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self):
'''
Constructor
'''
super(MoveArmState, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_config', 'group_name'])
self._action_topic = '/move_group'
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._joint_names = ['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3']
self._planning_failed = False
self._control_failed = False
self._success = False
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn('Move Action failed with result error code: %s' % str(result.error_code))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move Action failed with result error code: %s' % str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
action_goal = MoveGroupGoal()
action_goal.request.group_name = userdata.group_name
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_config[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e))
self._planning_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class Explore(EventState):
'''
Starts the Exploration Task via /move_base
># speed Speed of the robot
<= succeeded Exploration Task was successful
<= failed Exploration Task failed
'''
def __init__(self):
super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._succeeded = False
self._failed = False
def execute(self, userdata):
if self._move_client.has_result(self._action_topic):
result = self._move_client.get_result(self._action_topic)
self._dynrec.update_configuration({'speed':self._defaultspeed})
if result.result == 1:
self._reached = True
Logger.loginfo('Exploration succeeded')
return 'succeeded'
else:
self._failed = True
Logger.logwarn('Exploration failed!')
return 'failed'
def on_enter(self, userdata):
speedValue = self._dynrec.get_configuration(timeout = 0.5)
if speedValue is not None:
self._defaultspeed = speedValue['speed']
self._dynrec.update_configuration({'speed':userdata.speed})
self._succeeded = False
self._failed = False
action_goal = MoveBaseGoal()
action_goal.exploration = True
action_goal.speed = userdata.speed
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
if self._move_client.is_active(self._action_topic):
self._move_client.cancel(self._action_topic)
self._move_client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to start Exploration' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
def on_exit(self, userdata):
self._move_client.cancel(self._action_topic)
def on_start(self):
pass
def on_stop(self):
pass
class MoveArmDynState(EventState):
'''
Lets the robot move its arm.
># object_pose PoseStamped Pose of the object to observe.
># object_type string Object type.
># object_id string ID of the object.
<= reached Target joint configuration has been reached.
<= sampling_failed Failed to find any valid and promising camera pose.
<= planning_failed Failed to find a plan to move the arm to a desired camera pose.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self):
'''
Constructor
'''
super(MoveArmDynState, self).__init__(outcomes=['reached', 'sampling_failed', 'planning_failed', 'control_failed'],
input_keys=['object_pose', 'object_type', 'object_id'])
self._action_topic = '/combined_planner'
self._client = ProxyActionClient({self._action_topic: ArgoCombinedPlanAction})
# rotations for different object types to get z-axis aligned with optical axis
self._rot = tf.transformations.quaternion_about_axis(-0.5*math.pi, (0,1,0))
self._sampling_failed = False
self._planning_failed = False
self._control_failed = False
self._success = False
def execute(self, userdata):
'''
Execute this state
'''
if self._sampling_failed:
return 'sampling_failed'
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.success.val == ErrorCodes.SUCCESS:
self._success = True
return 'reached'
elif result.success.val == ErrorCodes.SAMPLING_FAILED:
Logger.logwarn('Sampling failed when attempting to move the arm')
self._sampling_failed = True
return 'sampling_failed'
elif result.success.val == ErrorCodes.PLANNING_FAILED:
Logger.logwarn('Planning failed when attempting to move the arm')
self._planning_failed = True
return 'planning_failed'
else:
Logger.logwarn('Control failed when attempting to move the arm')
self._control_failed = True
return 'control_failed'
def on_enter(self, userdata):
self._sampling_failed = False
self._planning_failed = False
self._control_failed = False
self._success = False
action_goal = ArgoCombinedPlanGoal()
action_goal.target = deepcopy(userdata.object_pose)
type_map = {
'dial_gauge': ObjectTypes.DIAL_GAUGE,
'level_gauge': ObjectTypes.LEVEL_GAUGE,
'valve': ObjectTypes.VALVE,
'hotspot': ObjectTypes.HOTSPOT,
'pipes' : ObjectTypes.PIPES,
'victim' : ObjectTypes.VICTIM
}
object_type = type_map.get(userdata.object_type, ObjectTypes.UNKNOWN)
q = [ action_goal.target.pose.orientation.x, action_goal.target.pose.orientation.y,
action_goal.target.pose.orientation.z, action_goal.target.pose.orientation.w ]
q = tf.transformations.quaternion_multiply(q, self._rot)
action_goal.target.pose.orientation = Quaternion(*q)
action_goal.object_id.data = userdata.object_id
action_goal.object_type.val = object_type
action_goal.action_type.val = ActionCodes.SAMPLE_MOVE_ARM
Logger.loginfo('Position arm to look at %s object' % str(userdata.object_type))
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e))
self._control_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class ParameterActionClient(EventState):
'''
parameter_ac is an action client state that will make a call to an action server.
This is intended to serve as a template for future states that need to communicate with
action server. The topic given should correspond to an action server and it right now only
works for StageAction action messages but in the future it will have any custom messages needed
once they are more finalized. Currently just sends true or false to an action server and
either returns complete or failed based on the response.
TODO: Proper error checking other custom messages
<= continue All actions completed, data collection started
<= failed Data control failed to initialize or call something TODO: Proper error checking
'''
def __init__(self, topic):
# See example_state.py for basic explanations.
super(ParameterActionClient,
self).__init__(outcomes=['completed', 'failed'],
input_keys=["trial_params"])
self._topic = topic
self._client = ProxyActionClient({self._topic: TestParametersAction})
self._apparatus = None
# It may happen that the action client fails to send the action goal.
self._error = False
#temporary user interface for inputting test parameters
# self.trial_count = 0
# self.test_count = 0
# self.read_csv = False
# self.tests = []
# self.params = {
# "object" : 0,
# "angle" : 0,
# "trials" : 0
# }
def execute(self, userdata):
# Check if the client failed to send the goal.
if self._error:
return 'failed'
#otherwise get the result and perform next steps
if (self._client.has_result(self._topic)):
result = self._client.get_result(self._topic)
status = result.result
if (status == 0):
print("Completed Succesfully")
return 'completed'
else:
print("Error Thrown")
return 'failed'
def on_enter(self, userdata):
rospy.loginfo(" --------------------- {}".format(userdata))
#Creating the goal to send for testing
goal = TestParametersGoal()
goal.parameters = userdata.trial_params
# find which apparatus is being used
# if self._apparatus == None:
# if rosnode.rosnode_ping("testbed_controller", max_count=10):
# self._apparatus = "testbed"
# else:
# self._apparatus = "door/drawer"
# #for testbed. Name of file to be read: parameters.csv
# self.trial_count = self.trial_count + 1
# if self._apparatus == "testbed":
# if(not self.read_csv):
# with open('/home/testbed-tower/infrastructure_system/parameters.csv', mode='r') as f:
# reader = csv.reader(f, delimiter=' ', quotechar='|')
# for row in reader:
# parsed_row = row[0].split(",")
# self.params["object"] = float(parsed_row[0])
# self.params["angle"] = float(parsed_row[1])
# self.params["trials"] = float(parsed_row[2])
# self.tests.append(copy.deepcopy(self.params))
# self.read_csv = True
# current_test = self.tests[self.test_count]
# if(self.trial_count == current_test["trials"]):
# self.test_count = self.test_count + 1
# self.trial_count = 0
# goal.parameters = [current_test["object"], current_test["angle"]]
# #for door/drawer
# else:
# goal.parameters = [1,2,3]
#error checking in case communication cant be established
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn(
'Failed to send the start data collection command:\n%s' %
str(e))
self._error = True
def on_exit(self, userdata):
# Makes sure that the action is not running when leaving this state.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class TweetPictureState(EventState):
'''
State for tweeting a picture and text
'''
def __init__(self):
# See example_state.py for basic explanations.
super(TweetPictureState, self).__init__(outcomes = ['picture_tweeted', 'tweeting_failed','command_error'], input_keys = ['picture_path', 'tweet_text'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'strands_tweets'
self._client = ProxyActionClient({self._topic: SendTweetAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'command_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
if result.success:
return 'picture_tweeted'
else:
return 'tweeting_failed'
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
# Create the goal.
tweet = SendTweetGoal()
tweet.text = userdata.tweet_text
if len(userdata.tweet_text) > 140:
tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!'
tweet.with_photo = True
img = cv2.imread(userdata.picture_path)
bridge = CvBridge()
image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8")
tweet.photo = image_message
try:
self._client.send_goal(self._topic, tweet)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn('Failed to send the TweetPictureState command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class GotoSingleArmJointConfigState(EventState):
"""
Directly commands the trajectory/joint controllers to move a
single joint to the desired configuration.
-- target_config int Identifier of the pre-defined pose to be used.
-- arm_side string Arm side {left, right}
># current_config dict The current arm joint positions
joint_names string[] : joint_values[]
<= done Successfully executed the motion.
<= failed Failed to execute the motion.
"""
# Wrists
WRIST_CCW = 11
WRIST_CW = 12
# Forearms
# ...
def __init__(self, arm_side, target_config, time=2.0):
"""Constructor"""
super(GotoSingleArmJointConfigState, self).__init__(outcomes=["done", "failed"], input_keys=["current_config"])
if not rospy.has_param("behavior/robot_namespace"):
Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server")
return
self._robot = rospy.get_param("behavior/robot_namespace")
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
################################ ATLAS ################################
self._configs = dict()
self._configs["flor"] = dict()
self._configs["flor"]["left"] = {
11: {"joint_name": "l_arm_wry2", "joint_value": -2.5},
12: {"joint_name": "l_arm_wry2", "joint_value": +2.5},
}
self._configs["flor"]["right"] = {
11: {"joint_name": "r_arm_wry2", "joint_value": +2.5},
12: {"joint_name": "r_arm_wry2", "joint_value": -2.5},
}
################################ THOR #################################
self._configs["thor_mang"] = dict()
self._configs["thor_mang"]["left"] = {
11: {"joint_name": "l_wrist_yaw2", "joint_value": 3.84},
12: {"joint_name": "l_wrist_yaw2", "joint_value": -3.84},
}
self._configs["thor_mang"]["right"] = {
11: {"joint_name": "r_wrist_yaw2", "joint_value": -3.84},
12: {"joint_name": "r_wrist_yaw2", "joint_value": 3.84},
}
#######################################################################
self._joint_name = self._configs[self._robot][arm_side][target_config]["joint_name"]
self._joint_value = self._configs[self._robot][arm_side][target_config]["joint_value"]
self._time = time
self._action_topic = (
"/" + controller_namespace + "/" + arm_side + "_arm_traj_controller" + "/follow_joint_trajectory"
)
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
def execute(self, userdata):
"""Execute this state"""
if self._failed:
return "failed"
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result:
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return "done"
else:
Logger.logwarn(
"Joint trajectory failed to execute (%d). Reason: %s" % (result.error_code, result.error_string)
)
self._failed = True
return "failed"
else:
Logger.logwarn("Wait for result returned True even though the result is %s" % str(result))
self._failed = True
return "failed"
def on_enter(self, userdata):
"""On enter, create and send the follow joint trajectory action goal."""
self._failed = False
current_config = userdata.current_config
# Get the index of the joint whose value will be replaced
index_of_joint = current_config["joint_names"].index(self._joint_name)
# Replace the old joint value with the target_config's one
new_values = current_config["joint_values"]
new_values[index_of_joint] = self._joint_value
# Create trajectory point out of the revised joint values
point = JointTrajectoryPoint(positions=new_values, time_from_start=rospy.Duration.from_sec(self._time))
# Create trajectory message
trajectory = JointTrajectory(joint_names=current_config["joint_names"], points=[point])
action_goal = FollowJointTrajectoryGoal(trajectory=trajectory)
# execute the motion
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn("Failed to send trajectory action goal:\n%s" % str(e))
self._failed = True
def on_exit(self, userdata):
"""Destructor"""
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class MoveToWaypointState(EventState):
'''
Lets the robot move to a given waypoint.
># waypoint PoseStamped Specifies the waypoint to which the robot should move.
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
'''
def __init__(self):
'''
Constructor
'''
super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed'],
input_keys=['waypoint'])
self._action_topic = '/move_base'
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._failed = False
self._reached = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.result == 1:
self._reached = True
return 'reached'
else:
self._failed = True
Logger.logwarn('Failed to reach waypoint!')
return 'failed'
def on_enter(self, userdata):
self._failed = False
self._reached = False
action_goal = MoveBaseGoal()
action_goal.target_pose = userdata.waypoint
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
Logger.loginfo('Driving to next waypoint')
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class ApproachPersonState(EventState):
'''
Actionlib actions are the most common basis for state implementations
since they provide a non-blocking, high-level interface for robot capabilities.
The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib).
This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state.
-- dishes_to_do int Expected amount of dishes to be cleaned.
># dishwasher int ID of the dishwasher to be used.
#> cleaned int Amount of cleaned dishes.
<= cleaned_some Only a few dishes have been cleaned.
<= cleaned_enough Cleaned a lot of dishes.
<= command_error Cannot send the action goal.
'''
def __init__(self):
# See example_state.py for basic explanations.
super(ApproachPersonState, self).__init__(outcomes = ['goal_reached', 'goal_failed','command_error'], input_keys = ['person_pose'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'stalk_pose'
self._client = ProxyActionClient({self._topic: StalkPoseAction}) # pass required clients as dict (topic: type)
self._timeoutTime = None
self._personTopic = '/upper_body_detector/closest_bounding_box_centre'
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
print 'approach_person_state: returning goal_reached'
return 'goal_reached'
# Check if the client failed to send the goal.
if self._error:
return 'command_error'
if rospy.Time.now() > self._timeoutTime:
return 'goal_failed'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
if result.success:
return 'goal_reached'
else:
return 'goal_failed'
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
# Create the goal.
goal = StalkPoseGoal()
goal.runtime_sec = 60
goal.topic_name = self._personTopic
goal.target = userdata.person_pose
self._error = False
self._timeoutTime = rospy.Time.now() + rospy.Duration(20)
#try:
# self._client.send_goal(self._topic, goal)
#except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
# Logger.logwarn('Failed to send the ApproachPerson command:\n%s' % str(e))
# self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class LookAtPattern(EventState):
'''
Specify a pattern for the robots camera to follow
># pattern string The pattern to follow
<= succeeded Camera follows the pattern
<= failed Failed to set pattern
'''
def __init__(self):
'''
Constructor
'''
super(LookAtPattern, self).__init__(outcomes=['succeeded', 'failed'],
input_keys=['pattern'])
self._action_topic = '/pan_tilt_sensor_head_joint_control/look_at'
self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction})
self._failed = False
self._succeeded = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._succeeded:
return 'succeeded'
def on_enter(self, userdata):
self._failed = False
self._succeeded = False
action_goal = hector_perception_msgs.msg.LookAtGoal()
action_goal.look_at_target.pattern = userdata.pattern
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send LookAt request with pattern %(pat)s\n%(err)s' % {
'pat': userdata.pattern,
'err': str(e)
})
self._failed = True
Logger.loginfo('Pattern %(pat)s loaded' % { 'pat': userdata.pattern })
self._succeeded = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class GripperRollState(EventState):
"""
Open or close the gripper.
-- rotation float Rotation of the joint, use HORIZONTAL for horizontal pose (0).
-- duration float Time (sec) for executing the motion.
<= done Trajectory has successfully finished its execution.
<= failed Failed to execute trajectory.
"""
HORIZONTAL = 0
def __init__(self, rotation, duration=1.0):
"""
Constructor
"""
super(GripperRollState, self).__init__(outcomes=["done", "failed"])
self._action_topic = "/gripper_control/gripper_roll_traj_controller/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
self._duration = duration
self._target_joint_value = rotation
def execute(self, userdata):
if self._failed:
return "failed"
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result:
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return "done"
else:
Logger.logwarn(
"Joint trajectory request failed to execute (%d). Reason: %s"
% (result.error_code, result.error_string)
)
self._failed = True
return "failed"
else:
Logger.logwarn("Wait for result returned True even though the result is %s" % str(result))
self._failed = True
return "failed"
def on_enter(self, userdata):
self._failed = False
# create point
endpoint = JointTrajectoryPoint()
endpoint.positions.append(self._target_joint_value)
endpoint.time_from_start = rospy.Duration(0.2 + self._duration)
# create goal
goal = FollowJointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
goal.trajectory.joint_names.append("arm_joint_4")
goal.trajectory.points.append(endpoint)
# execute the motion
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Was unable to execute joint trajectory:\n%s" % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class GripperState(EventState):
'''
Open or close the gripper.
-- close_fraction float OPEN, CLOSE or any value in [0,1].
-- duration float Time (sec) for executing the motion.
<= done Trajectory has successfully finished its execution.
<= failed Failed to execute trajectory.
'''
OPEN = 0
CLOSE = 1
def __init__(self, action, duration = 1.0):
'''
Constructor
'''
super(GripperState, self).__init__(outcomes=['done', 'failed'])
self._action_topic = "/gripper_control/gripper_grasp_traj_controller/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
self._duration = duration
self._target_joint_value = action * 1.89
def execute(self, userdata):
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result:
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return 'done'
else:
Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string))
self._failed = True
return 'failed'
else:
Logger.logwarn('Wait for result returned True even though the result is %s' % str(result))
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
# create point
endpoint = JointTrajectoryPoint()
endpoint.positions.append(self._target_joint_value)
endpoint.time_from_start = rospy.Duration(0.2 + self._duration)
# create goal
goal = FollowJointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
goal.trajectory.joint_names.append('gripper_joint_0')
goal.trajectory.points.append(endpoint)
# execute the motion
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class PlanFootstepsState(EventState):
'''
Creates a footstep plan to reach the desired feet pose via the OBFSM.
-- mode string One of the available planning modes (class constants).
># step_goal Feet Desired feet pose.
#> plan_header Header The header of the footstep plan.
<= planned Successfully created a plan.
<= failed Failed to create a plan.
'''
MODE_STEP_NO_COLLISION = 'drc_step_no_collision'
MODE_STEP_2D = 'drc_step_2D'
MODE_STEP_3D = 'drc_step_3D'
MODE_WALK = 'drc_walk'
def __init__(self, mode):
'''
Constructor
'''
super(PlanFootstepsState,
self).__init__(outcomes=['planned', 'failed'],
input_keys=['step_goal'],
output_keys=['plan_header'])
self._action_topic = "/vigir/footstep_manager/step_plan_request"
self._client = ProxyActionClient(
{self._action_topic: StepPlanRequestAction})
self._mode = mode
self._done = False
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
userdata.plan_header = None
return 'failed'
if self._done:
return 'planned'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.status.warning != ErrorStatus.NO_WARNING:
Logger.logwarn('Planning footsteps warning:\n%s' %
result.status.warning_msg)
if result.status.error == ErrorStatus.NO_ERROR:
userdata.plan_header = result.step_plan.header
num_steps = len(result.step_plan.steps)
Logger.loginfo('Received plan with %d steps' % num_steps)
self._done = True
return 'planned'
else:
userdata.plan_header = None
Logger.logerr('Planning footsteps failed:\n%s' %
result.status.error_msg)
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Upon entering the state, send the footstep plan request.'''
self._failed = False
self._done = False
# Create request msg and action goal
request = StepPlanRequest()
request.header = userdata.step_goal.header
request.max_planning_time = 10.0
request.parameter_set_name = String(self._mode)
action_goal = StepPlanRequestGoal(plan_request=request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send footstep plan request:\n%s' %
str(e))
self._failed = True
def on_exit(self, userdata):
'''Destructor'''
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class ExecuteTrajectoryState(EventState):
'''
Executes a given joint trajectory message.
-- controller string Namespace of the joint trajectory controller.
The part of the topic name before /follow_joint_trajectory.
># joint_trajectory JointTrajectory Trajectory to be executed, containing all required information.
<= done Trajectory has successfully finished its execution.
<= failed Failed to execute trajectory.
'''
def __init__(self, controller):
'''
Constructor
'''
super(ExecuteTrajectoryState, self).__init__(outcomes=['done', 'failed'],
input_keys=['joint_trajectory'])
self._action_topic = controller + "/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result:
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return 'done'
else:
Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string))
self._failed = True
return 'failed'
else:
Logger.logwarn('Wait for result returned True even though the result is %s' % str(result))
self._failed = True
return 'failed'
def on_enter(self, userdata):
self._failed = False
# create goal
goal = FollowJointTrajectoryGoal()
goal.trajectory = userdata.joint_trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
# execute the motion
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class ExampleActionState(EventState):
'''
Actionlib actions are the most common basis for state implementations
since they provide a non-blocking, high-level interface for robot capabilities.
The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib).
This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state.
-- dishes_to_do int Expected amount of dishes to be cleaned.
># dishwasher int ID of the dishwasher to be used.
#> cleaned int Amount of cleaned dishes.
<= cleaned_some Only a few dishes have been cleaned.
<= cleaned_enough Cleaned a lot of dishes.
<= command_error Cannot send the action goal.
'''
def __init__(self, dishes_to_do):
# See example_state.py for basic explanations.
super(ExampleActionState, self).__init__(
outcomes=['cleaned_some', 'cleaned_enough', 'command_error'],
input_keys=['dishwasher'],
output_keys=['cleaned'])
self._dishes_to_do = dishes_to_do
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'do_dishes'
self._client = ProxyActionClient(
{self._topic:
DoDishesAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'command_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
dishes_cleaned = result.total_dishes_cleaned
# In this example, we also provide the amount of cleaned dishes as output key.
userdata.cleaned = dishes_cleaned
# Based on the result, decide which outcome to trigger.
if dishes_cleaned > self._dishes_to_do:
return 'cleaned_enough'
else:
return 'cleaned_some'
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
dishwasher_id = userdata.dishwasher
# Create the goal.
goal = DoDishesGoal()
goal.dishwasher_id = dishwasher_id
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn('Failed to send the DoDishes command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class HandTrajectoryState(EventState):
'''
Executes a given hand trajectory, i.e., a request to open or close the fingers.
-- hand_type string Type of hand (e.g. 'robotiq')
># finger_trajectory JointTrajectory A single joint trajectory for the hand joints.
># hand_side string Which hand side the trajectory refers to (e.g. 'left')
<= done The trajectory was successfully executed.
<= failed Failed to execute trajectory.
'''
def __init__(self, hand_type):
'''Constructor'''
super(HandTrajectoryState, self).__init__(outcomes=['done', 'failed'],
input_keys=['finger_trajectory', 'hand_side'])
if not rospy.has_param("behavior/hand_controllers_name"):
Logger.logerr("Need to specify parameter behavior/hand_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/hand_controllers_name")
if not rospy.has_param("behavior/hand_type_prefix"):
Logger.logerr("Need to specify parameter behavior/hand_type_prefix at the parameter server")
return
hand_type_prefix = rospy.get_param("behavior/hand_type_prefix")
# Determine which hand types and sides have been sourced
self._hands_in_use = list()
LEFT_HAND = os.environ[hand_type_prefix + 'LEFT_HAND_TYPE']
RIGHT_HAND = os.environ[hand_type_prefix + 'RIGHT_HAND_TYPE']
if LEFT_HAND == 'l_' + hand_type:
self._hands_in_use.append('left')
if RIGHT_HAND == 'r_' + hand_type:
self._hands_in_use.append('right')
if len(self._hands_in_use) == 0:
Logger.logerr('No %s hands seem to be in use:\nLEFT_HAND = %s\nRIGHT_HAND = %s' % (hand_type, LEFT_HAND, RIGHT_HAND))
# Initialize the action clients corresponding to the hands in use
self._client_topics = dict()
self._active_topic = None
self._client = ProxyActionClient()
for hand_side in self._hands_in_use:
action_topic = ("/%s/%s_hand_traj_controller/follow_joint_trajectory" % (controller_namespace, hand_side))
self._client.setupClient(action_topic, FollowJointTrajectoryAction)
self._client_topics[hand_side] = action_topic
self._failed = False
def execute(self, userdata):
'''During execution of the state, keep checking for action servers response'''
if self._failed:
return 'failed'
if self._client.has_result(self._active_topic):
result = self._client.get_result(self._active_topic)
# Logger.loginfo('Action server says: %s' % result.error_code)
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return 'done'
else:
Logger.logwarn('Hand trajectory request failed to execute: %s (%d)' % (result.error_string, result.error_code))
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Upon entering the state, create and send the action goal message'''
self._failed = False
if userdata.hand_side not in self._hands_in_use:
Logger.logerr('Hand side from userdata (%s) does not match hands in use: %s' % (userdata.hand_side, self._hands_in_use))
self._failed = True
return
# Create goal message
goal = FollowJointTrajectoryGoal()
goal.trajectory = userdata.finger_trajectory
# Send goal to action server for execution
try:
self._active_topic = self._client_topics[userdata.hand_side]
self._client.send_goal(self._active_topic, goal)
except Exception as e:
Logger.logwarn('Failed to send follow (hand) joint trajectory action goal:\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._active_topic):
self._client.cancel(self._active_topic)
Logger.loginfo("Cancelled active action goal.")
class MoveitToJointsState(EventState):
'''
Uses MoveIt to plan and move the specified joints to the target configuration.
-- move_group string Name of the move group to be used for planning.
Specified joint names need to exist in the given group.
-- joint_names string[] Names of the joints to set.
Does not need to specify all joints.
-- action_topic string Topic on which MoveIt is listening for action calls.
># joint_config float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self, move_group, joint_names, action_topic='/move_group'):
'''
Constructor
'''
super(MoveitToJointsState, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_config'])
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = move_group
self._joint_names = joint_names
self._planning_failed = False
self._control_failed = False
self._success = False
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn(
'Control failed for move action of group: %s (error code: %s)'
% (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn(
'Move action failed with result error code: %s' %
str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=userdata.joint_config[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class MoveitToPose(EventState):
'''
Uses MoveIt to plan and move the move group to the target pose.
`move_group` node and `robot_description` parameter must present in current namespace.
State depends only on `move_group` action interface.
NOTE: sometimes lower tolerance values (e.g. 0.01 m and 0.1 rad) may cause planner to fail.
-- move_group string Name of the move group to be used for planning.
-- plan_only boolean Do not perform any movements. Only plan trajectory.
-- position_tolerance float Target positon tolerance in meters (default: 0.001 m).
-- orientation_tolerance float Target orientation tolerance in radians (deafult: 0.1 (6 degree))
># pose object Desired pose as geomery_msgs.msg.PoseStamped message.
<= reached Target configuration has been reached (or can be reaced).
<= planning_failed Failed to find a plan to the given configuration.
<= control_failed Failed to move along the planned trajectory.
'''
def __init__(self,
move_group,
plan_only=False,
position_tolerance=0.001,
orientation_tolerance=0.001):
'''
Constructor
'''
super(MoveitToPose, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['pose'])
# store parameters and get end effector
self._move_group = move_group
self._position_tolerance = position_tolerance
self._orientation_tolerance = orientation_tolerance
self._plan_only = plan_only
self._client = ProxyActionClient({'move_group': MoveGroupAction})
# get end effector link
self._end_effector = None
srdf_param = rospy.get_param("robot_description_semantic")
srdf = ET.fromstring(srdf_param)
for ee in srdf.iter('end_effector'):
if ee.attrib['group'] == move_group:
self._end_effector = ee.attrib['parent_link']
break
if not self._end_effector:
Logger.logerr(
'MoveitToPose: Unable to determine end effector for group `%s`.'
% self._move_group)
raise RuntimeError(
'Unable to determine end effector for group `%s`.' %
self._move_group)
self._planning_failed = False
self._control_failed = False
self._success = False
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
# TODO check userdata
# if not isinstance(userdata.pose, PoseStamped):
#Logger.logwarn('userdata.pose must be geomery_msgs.msg.PoseStamped. `%s` received' % str(type(userdata.pose)))
#self._planning_failed = True
#return
check_type('pose', 'geometry_msgs/PoseStamped', userdata.pose)
# request planing and execution
action_goal = MoveGroupGoal()
# set palnning options
action_goal.planning_options.plan_only = self._plan_only
action_goal.planning_options.replan = False
# action_goal.planning_options.planning_scene_diff.is_diff = True
# action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
# setup request
action_goal.request.group_name = self._move_group
action_goal.request.num_planning_attempts = 3
action_goal.request.allowed_planning_time = 1.0
action_goal.request.max_velocity_scaling_factor = 1.0
action_goal.request.max_acceleration_scaling_factor = 1.0
# start pose
action_goal.request.start_state.is_diff = True
# pose constraint
goal_constraint = Constraints(name='')
# set target position
constraint = PositionConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.constraint_region = BoundingVolume()
constraint.constraint_region.primitives = [
SolidPrimitive(type=SolidPrimitive.SPHERE,
dimensions=[self._position_tolerance])
]
constraint.constraint_region.primitive_poses = [
Pose(position=userdata.pose.pose.position)
]
constraint.weight = 1.0
goal_constraint.position_constraints.append(constraint)
# set target orientation
constraint = OrientationConstraint()
constraint.header = Header(frame_id=userdata.pose.header.frame_id)
constraint.link_name = self._end_effector
constraint.orientation = userdata.pose.pose.orientation
constraint.absolute_x_axis_tolerance = self._orientation_tolerance
constraint.absolute_y_axis_tolerance = self._orientation_tolerance
constraint.absolute_z_axis_tolerance = self._orientation_tolerance
constraint.weight = 0.1
goal_constraint.orientation_constraints.append(constraint)
# add goal_constraint
action_goal.request.goal_constraints.append(goal_constraint)
try:
self._client.send_goal('move_group', action_goal)
except Exception as e:
Logger.logwarn(
'MoveitToPose: Failed to send MoveGroupAction goal for group: %s\n%s'
% (self._move_group, str(e)))
self._planning_failed = True
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result('move_group'):
result = self._client.get_result('move_group')
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn(
'MoveitToPose: Control failed for MoveGroupAction of group: %s (error code: %s)'
% (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn(
'MoveitToPose: Move action failed with result error code: %s'
% str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class TiltHeadState(EventState):
'''
Executes a given joint trajectory message.
-- desired_tilt float Degrees (positive is down, negative is up).
Class variables are available (UP, DOWN, STRAIGHT).
<= done The head was successfully tilted.
<= failed Failed to execute neck trajectory.
'''
UP_40 = -40.0 # max -40 degrees
UP_20 = -20.0
STRAIGHT = +0.00
DOWN_30 = +20.0
DOWN_45 = +40.0
DOWN_60 = +60.0 # max +60 degrees
# HEAD
# STRAIGHT = 0
# UP_40 = 1
# UP_20 = 2
# DOWN_30 = 3
# DOWN_45 = 4
# DOWN_60 = 5
def __init__(self, desired_tilt):
'''Constructor'''
super(TiltHeadState, self).__init__(outcomes=['done', 'failed'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
# self._configs['flor']['same'] = {
# 20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees
# 21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'},
# 22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'},
# 23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'},
# 24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'},
# 25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'} # max +60 degrees
# }
self._action_topic = "/" + controller_namespace + \
"/neck_traj_controller" + "/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
# Convert desired position to radians
self._desired_tilt = math.radians(desired_tilt)
self._failed = False
def execute(self, userdata):
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result:
if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
return 'done'
else:
Logger.logwarn('Joint trajectory request failed to execute: (%d) %s' % (result.error_code, result.error_string))
self._failed = True
return 'failed'
else:
Logger.logwarn('Wait for result returned True even though the result is %s' % str(result))
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Upon entering, create a neck trajectory and send the action goal.'''
self._failed = False
# Create neck point
neck_point = JointTrajectoryPoint()
neck_point.time_from_start = rospy.Duration.from_sec(1.0)
neck_point.positions.append(self._desired_tilt)
# Create neck trajectory
neck_trajectory = JointTrajectory()
neck_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
neck_trajectory.joint_names = ['neck_ry']
neck_trajectory.points.append(neck_point)
# Create action goal
action_goal = FollowJointTrajectoryGoal(trajectory = neck_trajectory)
# execute the motion
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send neck trajectory goal:\n%s' % str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class WallFollowingActionState(EventState):
'''
State for calling wall_following action server.
'''
def __init__(self, action_server_name, desired_yaw, desired_speed, desired_wall_distance):
# See example_state.py for basic explanations.
super(WallFollowingActionState, self).__init__( outcomes = ['successed', 'aborted', 'preempted'])
self._topic = action_server_name
self._desired_yaw = desired_yaw
self._desired_speed = desired_speed
self._desired_wall_distance = desired_wall_distance
self._client = ProxyActionClient({self._topic: FollowingAction}) # pass required clients as dict (topic: type)
Logger.loginfo('[WallFollowing]: Action client initialized')
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'aborted'
# Check if the action has been finished
if self._client.has_result(self._topic):
status = self._client.get_state(self._topic)
result = self._client.get_result(self._topic)
if status == GoalStatus.SUCCEEDED:
Logger.loginfo("[WallFollowing]: %s" % str(result.message))
return 'successed'
elif status == GoalStatus.PREEMPTED:
Logger.logwarn('[WallFollowing]: Action preempted')
return 'preempted'
elif status in [GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED]:
Logger.logerr('[WallFollowing]: Action failed: %s' % str(result.message))
return 'aborted'
"""feedback params are"""
feedback = self._client.get_feedback(self._topic)
if feedback is not None:
Logger.loginfo("[WallFollowing]: Current feedback msg: %s" % feedback.message)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = FollowingGoal()
#fill the goal
goal.yaw = self._desired_yaw
goal.wall_distance = self._desired_wall_distance
goal.speed = self._desired_speed
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logerr('[WallFollowing]: Failed to send the goal:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('[WallFollowing]: Cancelled active action goal.')
class ExecuteStepPlanActionState(EventState):
'''
Implements a state to execute a step plan of the footstep planner via the OBFSM.
This state will change the control mode of the robot to STEP during execution and expects to be in STAND when entered.
># plan_header Header Header of the footstep plan to be executed.
Use one of the footstep planning states to calculate such a plan.
<= finished Finished walking.
Control mode will be changed back to STAND, but this can take some time.
If you need to be in STAND for the next state, add a CheckCurrentControlModeState.
<= failed Failed to completely execute the plan. Plan might have been executed partially.
'''
def __init__(self):
'''
Constructor
'''
super(ExecuteStepPlanActionState , self).__init__(outcomes=['finished','failed'],
input_keys=['plan_header'])
self._action_topic = "/vigir/footstep_manager/execute_step_plan"
self._client = ProxyActionClient({self._action_topic: ExecuteStepPlanAction})
self._failed = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if (result is not None and
result.status.status == FootstepExecutionStatus.REACHED_GOAL):
return 'finished'
else:
if result is None:
Logger.logwarn("Got None as result")
else:
Logger.logwarn("Result status %d" % result.status.status)
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Upon entering the state, request step plan execution from OBFSM'''
execution_request = StepPlan(header = userdata.plan_header)
action_goal = ExecuteStepPlanGoal(step_plan = execution_request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Unable to execute step plan')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class MoveitToJointsDynState(EventState):
'''
Uses MoveIt to plan and move the specified joints to the target configuration.
-- move_group string Name of the move group to be used for planning.
Specified joint names need to exist in the given group.
-- action_topic string Topic on which MoveIt is listening for action calls.
># joint_names string[] Names of the joints to set.
Does not need to specify all joints.
># joint_values float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self, move_group, action_topic = '/move_group'):
'''
Constructor
'''
super(MoveitToJointsDynState, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_values', 'joint_names'])
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = move_group
self._joint_names = None
self._planning_failed = False
self._control_failed = False
self._success = False
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_names = userdata.joint_names
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class CreateStepGoalState(EventState):
'''
Creates a footstep goal from a desired pose.
-- pose_is_pelvis boolean Set this to True if the pose is given
as pelvis pose and not on the ground.
># target_pose PoseStamped Pose to which the robot should walk.
#> step_goal Feet Desired feet pose.
<= done Successfully created a step goal.
<= failed Failed to create a plan.
'''
def __init__(self, pose_is_pelvis = False):
'''Constructor'''
super(CreateStepGoalState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['target_pose'],
output_keys = ['step_goal'])
self._action_topic = "/vigir/footstep_manager/generate_feet_pose"
self._client = ProxyActionClient({self._action_topic: GenerateFeetPoseAction})
self._pose_is_pelvis = pose_is_pelvis
self._done = False
self._failed = False
def execute(self, userdata):
'''Execute this state'''
if self._failed:
return 'failed'
if self._done:
return 'done'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.status.error == ErrorStatus.NO_ERROR:
userdata.step_goal = result.feet
self._done = True
return 'done'
else:
Logger.logwarn('Step goal creation failed:\n%s' % result.status.error_msg)
self._failed = True
return 'failed'
def on_enter(self, userdata):
'''Upon entering the state, request the feet pose.'''
self._done = False
self._failed = False
# Create request msg and action goal
request = FeetPoseRequest()
request.header = userdata.target_pose.header
request.header.stamp = rospy.Time.now() # timestamp used to track goal
request.pose = userdata.target_pose.pose
request.flags = FeetPoseRequest.FLAG_PELVIS_FRAME if self._pose_is_pelvis else 0
action_goal = GenerateFeetPoseGoal(request = request)
# Send action goal
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send step goal request')
rospy.logwarn(str(e))
self._failed = True
def on_exit(self, userdata):
'''Destructor'''
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class MoveitToJointsDynState(EventState):
'''
Uses MoveIt to plan and move the specified joints to the target configuration.
-- move_group string Name of the move group to be used for planning.
Specified joint names need to exist in the given group.
-- offset float Some offset
-- tool_link string e.g. "vacuum_gripper1_suction_cup"
-- action_topic string Topic on which MoveIt is listening for action calls.
># joint_names string[] Names of the joints to set.
Does not need to specify all joints.
># joint_values float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self,
move_group,
offset,
tool_link,
action_topic='/move_group'):
'''
Constructor
'''
super(MoveitToJointsDynState, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_values', 'joint_names'])
self._offset = offset
self._tool_link = tool_link
self._action_topic = action_topic
self._fk_srv_topic = '/compute_fk'
self._cartesian_srv_topic = '/compute_cartesian_path'
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK})
self._cartesian_srv = ProxyServiceCaller(
{self._cartesian_srv_topic: GetCartesianPath})
self._current_group_name = move_group
self._joint_names = None
self._planning_failed = False
self._control_failed = False
self._success = False
self._traj_client = actionlib.SimpleActionClient(
'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._traj_client.wait_for_server()
self._traj_exec_result = None
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._traj_exec_result:
# look at our cached 'execute_trajectory' result to see whether
# execution was successful
result = self._traj_exec_result
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn(
'Control failed for move action of group: %s (error code: %s)'
% (self._current_group_name, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn(
'Move action failed with result error code: %s' %
str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_names = userdata.joint_names
self._initial_state = RobotState()
self._initial_state.joint_state.position = copy.deepcopy(
userdata.joint_values)
self._initial_state.joint_state.name = copy.deepcopy(self._joint_names)
#print self._initial_state.joint_state.name
#print self._initial_state.joint_state.position
self._srv_req = GetPositionFKRequest()
self._srv_req.robot_state = self._initial_state
self._srv_req.header.stamp = rospy.Time.now()
self._srv_req.header.frame_id = "world"
self._srv_req.fk_link_names = [self._tool_link]
try:
srv_result = self._fk_srv.call(self._fk_srv_topic, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call FK: ' + str(e))
self._planning_failed = True
return
grasp_pose = srv_result.pose_stamped[0].pose
grasp_pose_stamped = srv_result.pose_stamped[0]
# Create a pre-grasp approach pose with an offset of 0.3
pre_grasp_approach_pose = copy.deepcopy(grasp_pose_stamped)
pre_grasp_approach_pose.pose.position.z += self._offset + 0.3
# Create an object to MoveGroupInterface for the current robot.
self._mgi_active_robot = MoveGroupInterface(
self._current_group_name, self._current_group_name +
'_base_link') # TODO: clean up in on_exit
self._mgi_active_robot.moveToPose(pre_grasp_approach_pose,
self._tool_link)
# Use cartesian motions to pick the object.
cartesian_service_req = GetCartesianPathRequest()
cartesian_service_req.start_state.joint_state = rospy.wait_for_message(
self._current_group_name + '/joint_states',
sensor_msgs.msg.JointState)
cartesian_service_req.header.stamp = rospy.Time.now()
cartesian_service_req.header.frame_id = "world"
cartesian_service_req.link_name = self._tool_link
cartesian_service_req.group_name = self._current_group_name
cartesian_service_req.max_step = 0.01
cartesian_service_req.jump_threshold = 0
cartesian_service_req.avoid_collisions = True
grasp_pose.position.z += self._offset + 0.16 # this is basically the toolframe (with the box as the tool)
cartesian_service_req.waypoints.append(grasp_pose)
try:
cartesian_srv_result = self._cartesian_srv.call(
self._cartesian_srv_topic, cartesian_service_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call Cartesian: ' + str(e))
self._planning_failed = True
return
if cartesian_srv_result.fraction < 1.0:
Logger.logwarn('Cartesian failed. fraction: ' +
str(cartesian_srv_result.fraction))
self._planning_failed = True
return
traj_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
traj_goal.trajectory = cartesian_srv_result.solution
self._traj_client.send_goal(traj_goal)
self._traj_client.wait_for_result()
self._traj_exec_result = self._traj_client.get_result()
def on_exit(self, userdata):
pass
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class hsr_MoveitToPoseGoalActionNew(EventState):
'''
Uses MoveIt to plan and move the specified joints to the target configuration.
-- move_group string Name of the move group to be used for planning.Specified joint names need to exist in the given group.
-- action_topic string Topic on which MoveIt is listening for action calls.
-- orien_tolerance bool Default: True
># pose_goal pose Target configuration of the joints.
># plan_num int plan_num
#> move_status string move_status
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self, move_group='whole_body', action_topic = '/move_group', orien_tolerance=True):
'''
Constructor
'''
super(hsr_MoveitToPoseGoalActionNew, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'], output_keys=['move_status'], input_keys=['pose_goal','plan_num'])
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self.orien_tolerance = orien_tolerance
self._move_group = move_group
self._planning_failed = False
self._control_failed = False
self._success = False
self.count_num = None
self._tolerance = 0.01
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander(self._move_group)
self.planning_frame = group.get_planning_frame()
self.planning_frame = '/map'
self.eef_link = group.get_end_effector_link()
group_names = robot.get_group_names()
scene = moveit_commander.PlanningSceneInterface()
def execute(self, userdata):
'''
Execute this state
'''
if self._planning_failed:
userdata.move_status = 'fail_plan'
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self.count_num = userdata.plan_num
self._planning_failed = False
self._control_failed = False
self._success = False
print('planning_frame:', self.planning_frame)
print('eef_link:', self.eef_link)
exec_pose = userdata.pose_goal
action_goal = make_default_action_goal(self, exec_pose, orientation=self.orien_tolerance)
action_goal.planning_options.planning_scene_diff.is_diff = True
action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
action_goal.planning_options.plan_only = False
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class FollowPathState(EventState):
"""
Follows a plan to reach the desired goal point.
-- topic String The node to communicate with
># plan Path Desired Path to follow
<= done Successfully reached the goal
<= failed Failed to generate or follow a plan
<= preempted The state was preempted
"""
def __init__(self, topic):
"""
Constructor
"""
super(FollowPathState,
self).__init__(outcomes=['done', 'failed', 'preempted'],
input_keys=['plan'])
self._action_topic = topic
self._client = ProxyActionClient(
{self._action_topic: FollowPathAction})
def execute(self, userdata):
"""
Execute this state
"""
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
ProxyActionClient._result[
self.
_action_topic] = None # Clear to avoid spam if blocked by low autonomy level
if result.code == 0:
Logger.loginfo('%s Success!' % (self.name))
self._return = 'done'
elif result.code == 1:
Logger.logerr('%s Failure' % (self.name))
self._return = 'failed'
elif result.code == 2:
Logger.logerr('%s Preempted' % (self.name))
self._return = 'preempted'
else:
Logger.logerr('%s Unknown error' % (self.name))
self._return = 'failed'
# Return prior value if blocked
return self._return
def on_enter(self, userdata):
"""
On enter, send action goal
"""
self._return = None
result = FollowPathGoal(path=userdata.plan)
try:
Logger.loginfo('%s Following the path to victory!' % (self.name))
self._client.send_goal(self._action_topic, result)
except Exception as e:
Logger.logwarn('%s Failed to follow path: %s' %
(self.name, str(e)))
return 'failed'
def on_exit(self, userdata):
if self._action_topic in ProxyActionClient._result:
ProxyActionClient._result[self._action_topic] = None
if self._client.is_active(self._action_topic):
Logger.logerr('%s Canceling active goal' % (self.name))
self._client.cancel(self._action_topic)
class GetPlanningSceneState(EventState):
'''
State to get planning scene using GetPlanningSceneAction.
-- components uint32 Bitfield defining the relevant parts of planning scene that are of interest.
(default: 1023 - send everything )
-- timeout double How long to wait for planning scene.
(default: 5 seconds)
-- wait_duration double How long to wait for action server (seconds) (0.0 waits indefinitely)
-- action_topic string Topic name for the ApplyPlanningScene action server.
(default: None --> Use user data)
># action_topic string Topic name for the ApplyPlanningScene action server (if None )
#> scene PlanningScene Output scene
<= done Planning scene was successfully applied.
<= failed Applying planning scene failed.
'''
def __init__(self, components=1023, timeout=5.0, wait_duration=2.0, action_topic=None):
'''
Constructor
'''
super(GetPlanningSceneState, self).__init__(
input_keys=['action_topic'],
outcomes=['done', 'failed'],
output_keys=['scene'] )
self.client = None
self.components = components
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.given_action_topic = action_topic
if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient({self.given_action_topic: GetPlanningSceneAction},
self.wait_duration)
else:
self.given_action_topic = None
self.current_action_topic = self.given_action_topic
self.scene = list()
self.return_code = None
def execute(self, userdata):
'''
Execute this state
'''
if self.return_code is not None:
# Return any previously assigned return code if we are in pause
userdata.scene = self.scene
return self.return_code
if self.client.has_result(self.current_action_topic):
result = self.client.get_result(self.current_action_topic)
self.scene = result.scene
self.return_code = 'done'
userdata.scene = self.scene
Logger.loginfo("GetPlanningScene - %s request succeeded." % (self.name))
return self.return_code
elif self.client.get_state(self.current_action_topic) == GoalStatus.ABORTED:
# No result returned is returned for this action, so go by the client state
Logger.logwarn("GetPlanningScene - %s request aborted by get planning scene action server" % (self.name))
self.return_code = 'failed'
return self.return_code
elif self.client.get_state(self.current_action_topic) == GoalStatus.REJECTED:
# No result returned is returned for this action, so go by the client state
Logger.logwarn("GetPlanningScene - %s request rejected by get planning scene action server" % (self.name))
self.return_code = 'failed'
return self.return_code
elif (self.timeout_target is not None) and (rospy.Time.now() > self.timeout_target):
Logger.logwarn("GetPlanningScene - timeout waiting for %s to get planning scene (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec()))
self.return_code = 'failed'
return self.return_code
def on_enter(self, userdata):
self.return_code = None
self.scene = None
# Retrieve the relevant data
try :
if (self.given_action_topic is None):
self.current_action_topic = userdata.action_topic
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s - invalid user data parameters\n%s' % (self.current_action_topic, str(e)))
self.return_code = 'failed'
return
try:
if (self.client is None):
self.client = ProxyActionClient({self.current_action_topic: GetPlanningSceneAction},
self.wait_duration)
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e)))
self.return_code = 'failed'
return
try:
if not self.client.is_available(self.current_action_topic):
self.client.setup_action_client(self.current_action_topic, GetPlanningSceneAction, self.wait_duration)
if not self.setup_action_client.is_available(self.current_action_topic):
Logger.logerr( 'Action client is not available for %s' % (self.current_action_topic))
self.return_code = 'failed'
return
except Exception as e:
Logger.logwarn('Failed to setup the action client for %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
try:
# Action Initialization
action_goal = GetPlanningSceneGoal()
action_goal.components = PlanningSceneComponents()
action_goal.components.components = self.components
if (self.timeout_duration > rospy.Duration(0.0)):
self.timeout_target = rospy.Time.now() + self.timeout_duration
else:
self.timeout_target = None
self.client.send_goal(self.current_action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
def on_stop(self):
try:
if ( self.client.is_available(self.current_action_topic) \
and not self.client.has_result(self.current_action_topic) ):
# Cancel any active goals
self.client.cancel(self.current_action_topic)
except Exception as e:
# client already closed
Logger.logwarn('Action client already closed - %s\n%s' % (self.current_action_topic, str(e)))
def on_resume(self, userdata):
# If paused during state execution, then re-send the clear command
self.on_enter(userdata)
class PlanFootstepsState(EventState):
"""
Creates a footstep plan to reach the desired feet pose via the OBFSM.
-- mode string One of the available planning modes (class constants).
># step_goal Feet Desired feet pose.
#> plan_header Header The header of the footstep plan.
<= planned Successfully created a plan.
<= failed Failed to create a plan.
"""
MODE_STEP_NO_COLLISION = "drc_step_no_collision"
MODE_STEP_2D = "drc_step_2D"
MODE_STEP_3D = "drc_step_3D"
MODE_WALK = "drc_walk"
def __init__(self, mode):
"""
Constructor
"""
super(PlanFootstepsState, self).__init__(
outcomes=["planned", "failed"], input_keys=["step_goal"], output_keys=["plan_header"]
)
self._action_topic = "/vigir/footstep_manager/step_plan_request"
self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction})
self._mode = mode
self._done = False
self._failed = False
def execute(self, userdata):
"""
Execute this state
"""
if self._failed:
userdata.plan_header = None
return "failed"
if self._done:
return "planned"
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.status.warning != ErrorStatus.NO_WARNING:
Logger.logwarn("Planning footsteps warning:\n%s" % result.status.warning_msg)
if result.status.error == ErrorStatus.NO_ERROR:
userdata.plan_header = result.step_plan.header
num_steps = len(result.step_plan.steps)
Logger.loginfo("Received plan with %d steps" % num_steps)
self._done = True
return "planned"
else:
userdata.plan_header = None
Logger.logerr("Planning footsteps failed:\n%s" % result.status.error_msg)
self._failed = True
return "failed"
def on_enter(self, userdata):
"""Upon entering the state, send the footstep plan request."""
self._failed = False
self._done = False
# Create request msg and action goal
request = StepPlanRequest()
request.header = userdata.step_goal.header
request.max_planning_time = 10.0
request.parameter_set_name = String(self._mode)
action_goal = StepPlanRequestGoal(plan_request=request)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn("Failed to send footstep plan request:\n%s" % str(e))
self._failed = True
def on_exit(self, userdata):
"""Destructor"""
if not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
Logger.loginfo("Cancelled active action goal.")
class AlignGateState(EventState):
'''
Actionlib actions are the most common basis for state implementations
since they provide a non-blocking, high-level interface for robot capabilities.
The example is based on the DoDishes-example of actionlib (see http://wiki.ros.org/actionlib).
This time we have input and output keys in order to specify the goal and possibly further evaluate the result in a later state.
'''
def __init__(self):
# See example_state.py for basic explanations.
super(AlignGateState,
self).__init__(outcomes=['success', 'command_error'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'align'
self._client = ProxyActionClient(
{self._topic:
AlignAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def execute(self, userdata):
# While this state is active, check if the action has been finished and evaluate the result.
# Check if the client failed to send the goal.
if self._error:
return 'command_error'
# Check if the action has been finished
if self._client.has_result(self._topic):
self.result = self._client.get_result(self._topic)
if self.result:
return 'success'
# If the action has not yet finished, no outcome will be returned and the state stays active.
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# As documented above, we get the specification of which dishwasher to use as input key.
# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
objectName = userdata.object
# Create the goal.
goal = getObjectAlignment()
goal.object = objectName
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
Logger.logwarn('Failed to send the Align command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
# Make sure that the action is not running when leaving this state.
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
class SrdfStateToMoveit(EventState):
'''
State to look up a pre-defined joint configuration from the SRDF file loaded in the parameter server (/robot_description_semantic)
and send it to MoveIt to plan and move.
-- config_name string Name of the joint configuration of interest.
-- move_group string Name of the move group to be used for planning.
-- action_topic string Topic on which MoveIt is listening for action calls.
-- robot_name string Optional name of the robot to be used.
If left empty, the first one found will be used
(only required if multiple robots are specified in the same file).
># joint_config float[] Target configuration of the joints.
Same order as their corresponding names in joint_names.
<= reached Target joint configuration has been reached.
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
'''
def __init__(self, config_name, move_group="", action_topic = '/move_group', robot_name=""):
'''
Constructor
'''
super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
def execute(self, userdata):
if self._param_error:
return 'param_error'
'''
Execute this state
'''
if self._planning_failed:
return 'planning_failed'
if self._control_failed:
return 'control_failed'
if self._success:
return 'reached'
if self._client.has_result(self._action_topic):
result = self._client.get_result(self._action_topic)
if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
self._control_failed = True
return 'control_failed'
elif result.error_code.val != MoveItErrorCodes.SUCCESS:
Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
self._planning_failed = True
return 'planning_failed'
else:
self._success = True
return 'reached'
def on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
#Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib['name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib['group'] #Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
return 'param_error'
try:
self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
return 'param_error'
#Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def on_stop(self):
try:
if self._client.is_available(self._action_topic) \
and not self._client.has_result(self._action_topic):
self._client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
