class ApplyPlanningSceneState(EventState):
'''
State to apply planning scene using ApplyPlanningSceneAction.
-- timeout double How long to wait for clearing map confirmation
(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 )
<= done Planning scene was successfully applied.
<= failed Applying planning scene failed.
'''
def __init__(self, timeout=5.0, wait_duration=5, action_topic=None):
'''
Constructor
'''
super(ApplyPlanningSceneState, self).__init__(
input_keys=['action_topic'],
outcomes=['done', 'failed'])
self.client = None
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.action_topic = action_topic
if (self.action_topic is not None) and (len(self.action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction},
self.wait_duration)
else:
self.action_topic = None # override ""
self.moveit_client = None
try:
self.moveit_client = ProxyMoveItClient(None)
except Exception as e:
Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e)))
self.success = None
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
return self.return_code
if self.client.has_result(self.action_topic):
result = self.client.get_result(self.action_topic)
Logger.loginfo('ApplyPlanningScene - %s' % str(self.client.get_goal_status_text(self.action_topic)))
if result.success:
Logger.loginfo('ApplyPlanningScene success! ')
self.return_code = 'done'
return self.return_code
else:
Logger.logwarn('ApplyPlanningScene failed ' )
self.return_code = 'failed'
return self.return_code
elif self.client.get_state(self.action_topic) == GoalStatus.ABORTED:
# No result returned is returned for this action, so go by the client state
userdata.success = self.success
Logger.logwarn("ApplyPlanningScene - %s request aborted by apply planning scene action server" % (self.name))
self.return_code = 'failed'
return self.return_code
elif self.client.get_state(self.action_topic) == GoalStatus.REJECTED:
# No result returned is returned for this action, so go by the client state
userdata.success = self.success
Logger.logwarn("ApplyPlanningScene - %s request rejected by apply planning scene action server" % (self.name))
self.return_code = 'failed'
return self.return_code
elif rospy.Time.now() > self.timeout_target:
userdata.success = self.success
Logger.logwarn("ApplyPlanningScene - timeout waiting for %s to apply 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
#Logger.logwarn("ApplyPlanningScene - %s (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec()))
def on_enter(self, userdata):
self.return_code = None
# Retrieve the relevant data
try :
if (self.action_topic is None):
self.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.action_topic, str(e)))
self.return_code = 'failed'
return
try:
if (self.client is None):
self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction},
self.wait_duration)
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.action_topic, str(e)))
self.return_code = 'failed'
return
if self.moveit_client is None:
try:
self.moveit_client = ProxyMoveItClient(None)
except Exception as e:
self.moveit_client = None
self.return_code = 'param_error'
Logger.logerr(str(e))
Logger.logwarn('No MoveItClient configured - state %s does not exist or is currently undefined!' % self.name)
return
try:
# Action Initialization
action_goal = ApplyPlanningSceneGoal()
action_goal.scene = self.moveit_client._planning_scene
self.client.send_goal(self.action_topic, action_goal)
self.timeout_target = rospy.Time.now() + self.timeout_duration
except Exception as e:
Logger.logwarn('Failed to send ApplyPlanningSceneGoal for group - %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
def on_stop(self):
try:
if ( self.client.is_available(self.action_topic) \
and not self.client.has_result(self.action_topic) ):
# Cancel any active goals
self.client.cancel(self.action_topic)
except Exception as e:
# client already closed
Logger.logwarn('Action client already closed - %s\n%s' % (self.action_topic, str(e)))
def on_pause(self):
pass
def on_resume(self, userdata):
# If paused during state execution, then re-send the clear command
self.on_enter(userdata)
class ExecuteKnownTrajectoryState(EventState):
'''
State to execute known RobotTrajectory from MoveIt planner using ExecuteKnownTrajectoryAction capability.
-- timeout float How long to wait for execution completion
(default: 5 seconds)
-- max_delay float Seconds that a trajectory may be delayed and still be sent to controller
(default: -1 ignore time and send trajectory)
-- wait_duration float How long to wait for action server (seconds) (0.0 waits indefinitely)
-- action_topic string Topic name for the ExecuteKnownTrajectory action server
(default: None --> Use user data)
># action_topic string Topic name for the ExecuteKnownTrajectory action server (if None )
># trajectory RobotTrajectory The trajectory to execute.
#> status_text string Status text reflecting outcome
#> goal_names string[] List of joint names for goal state
#> goal_values float[] List of joint values for goal state
<= done Successful.
<= failed Failed.
<= param_error Invalid parameters - no trajectory sent
'''
def __init__(self, timeout=5.0, max_delay=-1.0, wait_duration=5, action_topic=None):
'''
Constructor
'''
super(ExecuteKnownTrajectoryState, self).__init__(
input_keys=['action_topic', 'trajectory'],
outcomes=['done', 'failed', 'param_error'],
output_keys=['status_text', 'goal_names', 'goal_values'] )
self.action_client = None
self.goal_names = None
self.goal_values = None
self.timeout_duration = rospy.Duration(timeout)
self.timeout_target = None
self.wait_duration = wait_duration
self.given_action_topic = action_topic
self.max_delay = None
if max_delay > 0.0:
self.max_delay = rospy.Duration(max_delay)
try:
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.action_client = ProxyActionClient({self.given_action_topic: ExecuteKnownTrajectoryAction},
self.wait_duration)
else:
self.given_action_topic = None # override ""
except Exception as e:
Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e)))
self.current_action_topic = self.given_action_topic
self.status_text = None
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.status_text = self.status_text
userdata.goal_names = self.goal_names
userdata.goal_values = self.goal_values
return self.return_code
if self.action_client.has_result(self.current_action_topic):
result = self.action_client.get_result(self.current_action_topic)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
self.status_text = 'ExecuteKnownTrajectory %s success! ' % self.name
self.return_code = 'done'
elif result.error_code.val == MoveItErrorCodes.PREEMPTED:
self.status_text = 'ExecuteKnownTrajectory %s preempted '% self.name
self.return_code = 'failed'
elif result.error_code.val == MoveItErrorCodes.TIMED_OUT:
self.status_text = 'ExecuteKnownTrajectory %s timed out '% self.name
self.return_code = 'failed'
elif result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
self.status_text = 'ExecuteKnownTrajectory %s control failed '% self.name
self.return_code = 'failed'
else:
self.status_text = 'ExecuteKnownTrajectory %s failed ' % self.name
self.return_code = 'failed'
Logger.loginfo('ExecuteKnownTrajectory result for %s - %s\n%s' % (self.name, self.status_text, str(self.action_client.get_goal_status_text(self.current_action_topic))))
userdata.status_text = self.status_text
userdata.goal_names = self.goal_names
userdata.goal_values = self.goal_values
return self.return_code
elif self.action_client.get_state(self.current_action_topic) == GoalStatus.ABORTED:
# No result returned is returned for this action, so go by the client state
self.status_text = "ExecuteKnownTrajectory- %s request aborted by execute known trajectory action server" % (self.name)
self.return_code = 'failed'
elif self.action_client.get_state(self.current_action_topic) == GoalStatus.REJECTED:
# No result returned is returned for this action, so go by the client state
self.status_text = "ExecuteKnownTrajectory - %s request rejected by execute known trajectory action server" % (self.name)
self.return_code = 'failed'
elif (self.timeout_target is not None) and (rospy.Time.now() > self.timeout_target):
self.status_text = "ExecuteKnownTrajectory - timeout waiting for %s to execute known trajectory(%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec())
self.return_code = 'failed'
#Logger.logwarn("ExecuteKnownTrajectory - %s (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec()))
if (self.return_code is not None):
Logger.logwarn( self.status_text )
userdata.status_text = self.status_text
userdata.goal_names = self.goal_names
userdata.goal_values = self.goal_values
return self.return_code
def on_enter(self, userdata):
self.return_code = None
self.status_text = None
self.goal_names = None
self.goal_values = None
# Retrieve the relevant data
try :
if (self.given_action_topic is None):
self.current_action_topic = userdata.action_topic
except Exception as e:
self.status_text = 'Failed to set up the action client for %s - invalid user data parameters\n%s' % (self.current_action_topic, str(e))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
try:
if (self.action_client is None):
self.action_client = ProxyActionClient({self.current_action_topic: ExecuteKnownTrajectoryAction},
self.wait_duration)
if not self.action_client.is_available(self.current_action_topic):
# Try to re-connect if not available
self.action_client.setupClient(self.current_action_topic, ExecuteKnownTrajectoryAction, self.wait_duration)
if not self.action_client.is_available(self.current_action_topic):
self.status_text = '%s - no connection to %s available!' % (self.name, self.current_action_topic)
self.return_code = 'param_error'
Logger.loginfo(self.status_text)
return
except Exception as e:
self.status_text = 'Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
try:
# Action Initialization
action_goal = ExecuteKnownTrajectoryGoal()
action_goal.trajectory = userdata.trajectory
# Check that our trajectory is still fresh
elapsed = rospy.Time.now() - action_goal.trajectory.joint_trajectory.header.stamp
if self.max_delay is not None and elapsed > self.max_delay:
self.status_text = '%s - Stale trajectory - %f > %f - do not send action for %s' % (self.name, elapsed.to_sec(), self.max_delay.to_sec(), self.current_action_topic )
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
elif elapsed.to_sec() > 2.0:
Logger.logwarn("%s - trajectory older than %f - send anyway!" % (self.name, elapsed.to_sec()))
# Reset the timestamp before sending as we assume execute from begining in this state
action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time(0)
self.action_client.send_goal(self.current_action_topic, action_goal)
if (self.timeout_duration > rospy.Duration(0.0)):
self.timeout_target = rospy.Time.now() + action_goal.trajectory.joint_trajectory.points[-1].time_from_start + self.timeout_duration
else:
self.timeout_target = None
self.goal_names = action_goal.trajectory.joint_trajectory.joint_names
self.goal_values = action_goal.trajectory.joint_trajectory.points[-1].positions
except Exception as e:
self.status_text = 'Failed to send ExecuteKnownTrajectoryGoal for %s\n%s' % (self.name, str(e))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
def on_stop(self):
try:
if ( self.action_client.is_available(self.current_action_topic) \
and not self.action_client.has_result(self.current_action_topic) ):
# Cancel any active goals
self.action_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_pause(self):
try:
self.action_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 command
self.on_enter(userdata)
class JointValuesToMoveActionState(EventState):
'''
State to send a specified joint configuration to a specified MoveGroup move action.
This state attempts a plan and execute move to a specified move group action
interface using joint names and values specified in parameters;
the number of joint values must match the given joint names, and the joint
names should be consistent with the specified move group
This state uses the basic MoveGroupAction interface with minimal configuration.
-- action_topic string Topic on which MoveIt is listening for action calls.
(default: None - use userdata )
-- move_group string Name of the move group to be used for planning.
(default: None - use userdata )
-- joint_names string[] List of joint names
Does not need to set all joints in group.
Same order as their corresponding names in joint_values.
-- joint_values float[] List of joint values
Same order as their corresponding names in joint_names.
-- joint_tolerance float Joint tolerance (above and below) (default: 0.0 use srdf limits)
-- constraint_weight float How much to weight joint constraint relative to other constraints
-- allowed_planning_time float Allowable planning time (seconds)
-- wait_duration float How long to wait for action server (seconds)
(default: 0 seconds)
-- timeout float How long to wait for execution completion after planning
(default: 10 seconds)
># action_topic string Topic on which MoveIt is listening for action calls.
># move_group string Name of the move group to be used for planning.
Pass the configuration data along for potential re-use later
#> 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.
#> joint_names string[] Names of the target joints.
#> joint_values float[] Target configuration of the joints.
<= reached Target joint configuration has been reached.
<= param_error Invalid user data - cannot send plan request
<= planning_failed Failed to find a plan to the given joint configuration.
<= control_failed Failed to move the arm along the planned trajectory.
<= failed Failed to send goal
'''
def __init__(self,
joint_names,
joint_values,
move_group=None,
action_topic=None,
joint_tolerance=0.0,
constraint_weight=1.0,
allowed_planning_time=2.0,
wait_duration=0,
timeout=10.0):
'''
Constructor
'''
super(JointValuesToMoveActionState,
self).__init__(outcomes=[
'reached', 'param_error', 'planning_failed',
'control_failed', 'failed'
],
input_keys=['move_group', 'action_topic'],
output_keys=[
'status_text', 'move_group', 'action_topic',
'joint_names', 'joint_values'
])
self.client = None
self.given_move_group = move_group
self.given_action_topic = action_topic
self.joint_names = joint_names
self.joint_values = joint_values
self.joint_tolerance = joint_tolerance
self.constraint_weight = constraint_weight
self.allowed_planning_time = allowed_planning_time
self.wait_duration = wait_duration
self.return_code = None
self.status_text = None
self.timeout_duration = rospy.Duration(timeout)
self.timeout_target = None
if (self.given_action_topic
is not None) and (len(self.given_action_topic) > 0):
self.client = ProxyActionClient(
{self.given_action_topic: MoveGroupAction}, self.wait_duration)
else:
self.given_action_topic = None
if (self.given_move_group is None) or (len(self.given_move_group) < 1):
self.given_move_group = None
self.current_action_topic = self.given_action_topic
self.current_move_group = self.given_move_group
def execute(self, userdata):
'''
Execute this state
'''
if self.return_code is not None:
userdata.status_text = self.status_text
return self.return_code
if self.client.has_result(self.current_action_topic):
result = self.client.get_result(self.current_action_topic)
Logger.loginfo('Move Group - %s' % str(
self.client.get_goal_status_text(self.current_action_topic)))
if result.error_code.val == MoveItErrorCodes.SUCCESS:
self.status_text = 'Move Group success! - %s' % str(
result.error_code)
self.return_code = 'reached'
elif result.error_code.val == MoveItErrorCodes.PLANNING_FAILED:
self.status_text = 'Move Group planning failed with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif result.error_code.val == MoveItErrorCodes.INVALID_MOTION_PLAN:
self.status_text = 'Move Group invalid motion plan with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif result.error_code.val == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE:
self.status_text = 'Move Group invalid motion plan due to environment change with result error code - %s' % str(
result.error_code)
self.return_code = 'control_failed'
elif result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
self.status_text = 'Move Group control failed for move action of group - %s (error code - %s)' % (
self.current_move_group, str(result.error_code))
self.return_code = 'control_failed'
elif result.error_code.val == MoveItErrorCodes.UNABLE_TO_AQUIRE_SENSOR_DATA:
self.status_text = 'Move Group cannot acquire sensor data with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif result.error_code.val == MoveItErrorCodes.TIMED_OUT:
self.status_text = 'Move Group timeout with result error code - %s' % str(
result.error_code)
self.return_code = 'control_failed'
elif result.error_code.val == MoveItErrorCodes.PREEMPTED:
self.status_text = 'Move Group preempted with result error code - %s' % str(
result.error_code)
self.return_code = 'control_failed'
elif result.error_code.val == MoveItErrorCodes.START_STATE_IN_COLLISION:
self.status_text = 'Move Group start state in collision with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif result.error_code.val == MoveItErrorCodes.START_STATE_VIOLATES_PATH_CONSTRAINTS:
self.status_text = 'Move Group start state violates path constraints with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif ( (result.error_code.val >= MoveItErrorCodes.INVALID_GROUP_NAME) and \
(result.error_code.val <= MoveItErrorCodes.START_STATE_VIOLATES_PATH_CONSTRAINTS) ):
self.status_text = 'Move Group invalid request data with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif ( (result.error_code.val >= MoveItErrorCodes.SENSOR_INFO_STALE) and \
(result.error_code.val <= MoveItErrorCodes.FRAME_TRANSFORM_FAILURE) ):
self.status_text = 'Move Group - system data failure result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif result.error_code.val == MoveItErrorCodes.NO_IK_SOLUTION:
self.status_text = 'Move Group no IK solution with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
elif result.error_code.val == MoveItErrorCodes.FAILURE:
self.status_text = 'Move Group unknown failure with result error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
else:
self.status_text = 'Move Group - failed with unknown error code - %s' % str(
result.error_code)
self.return_code = 'planning_failed'
Logger.loginfo(self.status_text)
userdata.status_text = self.status_text
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
self.status_text = "MoveGroup- %s request aborted by action server" % (
self.name)
self.return_code = 'failed'
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
self.status_text = "MoveGroup - %s request rejected by action server" % (
self.name)
self.return_code = 'failed'
elif (self.timeout_target
is not None) and (rospy.Time.now() > self.timeout_target):
self.status_text = "MoveGroup - timeout waiting for %s response (%f > %f (%f))" % (
self.name, rospy.Time.now().to_sec(),
self.timeout_target.to_sec(), self.timeout_duration.to_sec())
self.return_code = 'failed'
#Logger.logwarn("ExecuteKnownTrajectory - %s (%f > %f (%f))" % (self.name, rospy.Time.now().to_sec(), self.timeout_target.to_sec(), self.timeout_duration.to_sec()))
if (self.return_code is not None):
Logger.logwarn(self.status_text)
userdata.status_text = self.status_text
return self.return_code
def on_enter(self, userdata):
self.return_code = None
# Retrieve the relevant data
try:
if (len(self.joint_values) != len(self.joint_names)):
Logger.logwarn(
'Mismatch in joint names and values (%d vs. %d) -' %
(len(self.joint_values), len(self.joint_names)))
self.return_code = 'param_error'
return
except Exception as e:
Logger.logwarn('Failed to get relevant user data -\n%s' % (str(e)))
self.return_code = 'param_error'
return
try:
if (self.given_move_group is None):
self.current_move_group = userdata.move_group
if (self.given_action_topic is None):
self.current_action_topic = userdata.action_topic
if (self.client is None):
self.client = ProxyActionClient(
{self.current_action_topic: MoveGroupAction},
self.wait_duration)
if (not self.client.is_available(self.current_action_topic)):
Logger.logwarn('Action client is not available for %s' %
(self.current_action_topic))
self.return_code = 'param_error'
return
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 = 'param_error'
return
try:
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.start_state.is_diff = True # Flags start state as empty to use current state on server
action_goal.request.group_name = self.current_move_group
action_goal.request.allowed_planning_time = self.allowed_planning_time
action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server
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_values[i],
tolerance_above=self.joint_tolerance,
tolerance_below=self.joint_tolerance,
weight=self.constraint_weight))
action_goal.request.goal_constraints.append(goal_constraints)
self.client.send_goal(self.current_action_topic, action_goal)
if (self.timeout_duration > rospy.Duration(0.0)):
self.timeout_target = rospy.Time.now() + rospy.Duration(
self.allowed_planning_time) + self.timeout_duration
else:
self.timeout_target = None
# Pass the configuration data along as user data
userdata.move_group = self.current_move_group
userdata.action_topic = self.current_action_topic
userdata.joint_names = self.joint_names
userdata.joint_values = self.joint_values
except Exception as e:
Logger.logwarn('Failed to send action goal for group - %s\n%s' %
(self.current_move_group, str(e)))
self.return_code = 'planning_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_pause(self):
try:
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):
self.on_enter(userdata)
