def _monitoring_execute(self, *args, **kwargs):
new_status = None
had_warning = False
if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic):
new_status = ""
diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
for status in diag_msg.status:
if not status.name in self._monitored_keys.keys(): continue
if status.level == DiagnosticStatus.WARN:
had_warning = True
if not status.name + "_warn" in self._sent_keys:
self._sent_keys.append(status.name + "_warn")
Logger.logwarn("%s: %s" % (status.name, status.message))
if status.level == DiagnosticStatus.ERROR:
if not status.name + "_err" in self._sent_keys:
self._sent_keys.append(status.name + "_err")
Logger.logerr("%s: %s" % (status.name, status.message))
new_status = status.name
if new_status == "":
self._current_status = None
new_status = None
if not had_warning:
self._sent_keys = list()
if new_status is None or self._current_status == new_status:
return self.__execute(*args, **kwargs)
self._current_status = new_status
return self._monitored_keys[self._current_status]
def _mirror_structure_callback(self, msg):
Logger.localinfo("--> Creating behavior structure for mirror...")
self._pub.publish('flexbe/mirror/structure',
self._build_structure_msg())
Logger.localinfo("<-- Sent behavior structure for mirror.")
# enable control of states since a mirror is listening
self._enable_ros_control()
def _execute_current_state(self):
# catch any exception and keep state active to let operator intervene
try:
outcome = super(OperatableStateMachine,
self)._execute_current_state()
self._last_exception = None
except Exception as e:
outcome = None
self._last_exception = e
Logger.logerr('Failed to execute state %s:\n%s' %
(self.current_state_label, str(e)))
# provide explicit sync as back-up functionality
# should be used only if there is no other choice
# since it requires additional 8 byte + header update bandwith and time to restart mirror
if self._inner_sync_request and self.get_deep_state() is not None:
self._inner_sync_request = False
if self.id is None:
self.parent._inner_sync_request = True
else:
msg = BehaviorSync()
msg.behavior_id = self.id
msg.current_state_checksum = zlib.adler32(
self.get_deep_state().path.encode()) & 0x7fffffff
self._pub.publish('flexbe/mirror/sync', msg)
return outcome
def _monitoring_execute(self, *args, **kwargs):
new_status = None
had_warning = False
if (self._force_monitoring
or self._is_controlled) and self._sub.has_buffered(
self._diagnostics_topic):
new_status = ""
diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
for status in diag_msg.status:
if not status.name in self._monitored_keys.keys(): continue
if status.level == DiagnosticStatus.WARN:
had_warning = True
if not status.name + "_warn" in self._sent_keys:
self._sent_keys.append(status.name + "_warn")
Logger.logwarn("%s: %s" %
(status.name, status.message))
if status.level == DiagnosticStatus.ERROR:
if not status.name + "_err" in self._sent_keys:
self._sent_keys.append(status.name + "_err")
Logger.logerr("%s: %s" % (status.name, status.message))
new_status = status.name
if new_status == "":
self._current_status = None
new_status = None
if not had_warning:
self._sent_keys = list()
if new_status is None or self._current_status == new_status:
return self.__execute(*args, **kwargs)
self._current_status = new_status
return self._monitored_keys[self._current_status]
def setupClient(self, topic, msg_type, wait_duration=10):
"""
Tries to set up an action client for calling it later.
@type topic: string
@param topic: The topic of the action to call.
@type msg_type: msg type
@param msg_type: The type of messages of this action client.
@type wait_duration: int
@param wait_duration: Defines how long to wait for the given client if it is not available right now.
"""
if topic not in ProxyActionClient._clients:
client = actionlib.SimpleActionClient(topic, msg_type)
t = Timer(1, self._print_wait_warning, [topic])
t.start()
available = client.wait_for_server(
rospy.Duration.from_sec(wait_duration))
warning_sent = False
try:
t.cancel()
except Exception as ve:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Action client %s timed out!" % topic)
else:
ProxyActionClient._clients[topic] = client
if warning_sent:
Logger.loginfo("Finally found action client %s..." %
(topic))
def setupClient(self, topic, msg_type, wait_duration=10):
"""
Tries to set up an action client for calling it later.
@type topic: string
@param topic: The topic of the action to call.
@type msg_type: msg type
@param msg_type: The type of messages of this action client.
@type wait_duration: int
@param wait_duration: Defines how long to wait for the given client if it is not available right now.
"""
if topic not in ProxyActionClient._clients:
client = actionlib.SimpleActionClient(topic, msg_type)
t = Timer(1, self._print_wait_warning, [topic])
t.start()
available = client.wait_for_server(rospy.Duration.from_sec(wait_duration))
warning_sent = False
try:
t.cancel()
except Exception as ve:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Action client %s timed out!" % topic)
else:
ProxyActionClient._clients[topic] = client
if warning_sent:
Logger.loginfo("Finally found action client %s..." % (topic))
def _set_autonomy_level(self, msg):
""" Sets the current autonomy level. """
if OperatableStateMachine.autonomy_level != msg.data:
Logger.localinfo('--> Autonomy changed to %d' % msg.data)
if msg.data < 0:
self.preempt()
else:
OperatableStateMachine.autonomy_level = msg.data
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="autonomy", args=[]))
def add_joint_values(self, joint_values):
joint_names = ProxyMoveitClient._robot.get_joint_names(self._move_group)
if len(joint_names) != len(joint_values):
Logger.logwarn("Amount of given joint values (%d) does not match the amount of joints (%d)." % (len(joint_values), len(joint_names)))
#return
goal_constraints = Constraints()
for i in range(len(joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=joint_names[i], position=joint_values[i]))
self._goal.request.goal_constraints.append(goal_constraints)
def has_topic(self, topic):
"""
Determines if the given topic is already subscribed.
@type topic: string
@param topic: The topic of interest.
"""
Logger.logwarn(
'Deprecated (ProxySubscriberCached): use "is_available(topic)" instead of "has_topic(topic)".'
)
return self.is_available(topic)
def _sync_callback(self, msg):
Logger.localinfo("--> Synchronization requested...")
msg = BehaviorSync()
msg.behavior_id = self.id
# make sure we are already executing
self.wait(condition=lambda: self.get_deep_state() is not None)
msg.current_state_checksum = zlib.adler32(
self.get_deep_state().path.encode()) & 0x7fffffff
self._pub.publish('flexbe/mirror/sync', msg)
self._pub.publish('flexbe/command_feedback',
CommandFeedback(command="sync", args=[]))
Logger.localinfo("<-- Sent synchronization message for mirror.")
def _attach_callback(self, msg):
Logger.localinfo("--> Enabling control...")
# set autonomy level
OperatableStateMachine.autonomy_level = msg.data
# enable control of states
self._enable_ros_control()
self._inner_sync_request = True
# send command feedback
cfb = CommandFeedback(command="attach")
cfb.args.append(self.name)
self._pub.publish('flexbe/command_feedback', cfb)
Logger.localinfo("<-- Sent attach confirm.")
def __init__(self):
if not ProxyMoveitClient._is_initialized:
ProxyMoveitClient._is_initialized = True
Logger.loginfo("Initializing proxy MoveIt client...")
moveit_commander.roscpp_initialize(sys.argv)
ProxyMoveitClient._robot = moveit_commander.RobotCommander()
ProxyMoveitClient._client = ProxyActionClient({ProxyMoveitClient._action_topic: MoveAction})
self._goal = None
self._move_group = ""
self._result = None
self._planner_id = "RRTConnectkConfigDefault"
self._target_link_axis = []
def _operatable_execute(self, *args, **kwargs):
outcome = self.__execute(*args, **kwargs)
if self._is_controlled:
# reset previously requested outcome if applicable
if self._last_requested_outcome is not None and outcome is None:
self._pub.publish(
self._request_topic,
OutcomeRequest(outcome=255, target=self.path))
self._last_requested_outcome = None
# request outcome because autonomy level is too low
if not self._force_transition and (
not self.parent.is_transition_allowed(self.name, outcome)
or outcome is not None and self.is_breakpoint):
if outcome != self._last_requested_outcome:
self._pub.publish(
self._request_topic,
OutcomeRequest(outcome=self.outcomes.index(outcome),
target=self.path))
Logger.localinfo("<-- Want result: %s > %s" %
(self.name, outcome))
StateLogger.log(
'flexbe.operator',
self,
type='request',
request=outcome,
autonomy=self.parent.autonomy_level,
required=self.parent.get_required_autonomy(outcome))
self._last_requested_outcome = outcome
outcome = None
# autonomy level is high enough, report the executed transition
elif outcome is not None and outcome in self.outcomes:
Logger.localinfo("State result: %s > %s" %
(self.name, outcome))
self._pub.publish(self._outcome_topic,
UInt8(self.outcomes.index(outcome)))
self._pub.publish(self._debug_topic,
String("%s > %s" % (self.path, outcome)))
if self._force_transition:
StateLogger.log('flexbe.operator',
self,
type='forced',
forced=outcome,
requested=self._last_requested_outcome)
self._last_requested_outcome = None
self._force_transition = False
return outcome
def _preemptable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_msg(self._preempt_topic):
self._sub.remove_last_msg(self._preempt_topic)
self._pub.publish(self._feedback_topic,
CommandFeedback(command="preempt"))
PreemptableState.preempt = True
Logger.localinfo("--> Behavior will be preempted")
if PreemptableState.preempt:
if not self._is_controlled:
Logger.localinfo("Behavior will be preempted")
self._force_transition = True
return self._preempted_name
return self.__execute(*args, **kwargs)
def __init__(self):
if not ProxyMoveitClient._is_initialized:
ProxyMoveitClient._is_initialized = True
Logger.loginfo("Initializing proxy MoveIt client...")
moveit_commander.roscpp_initialize(sys.argv)
ProxyMoveitClient._robot = moveit_commander.RobotCommander()
ProxyMoveitClient._client = ProxyActionClient(
{ProxyMoveitClient._action_topic: MoveAction})
self._goal = None
self._move_group = ""
self._result = None
self._planner_id = "RRTConnectkConfigDefault"
self._target_link_axis = []
def get_behavior(self, be_id):
"""
Provides the library entry corresponding to the given ID.
@type be_id: int
@param be_id: Behavior ID to look up.
@return Corresponding library entry or None if not found.
"""
try:
return self._behavior_lib[be_id]
except KeyError:
Logger.logwarn("Did not find ID %d in libary, updating..." % be_id)
self.parse_packages()
return self._behavior_lib.get(be_id, None)
def add_joint_values(self, joint_values):
joint_names = ProxyMoveitClient._robot.get_joint_names(
self._move_group)
if len(joint_names) != len(joint_values):
Logger.logwarn(
"Amount of given joint values (%d) does not match the amount of joints (%d)."
% (len(joint_values), len(joint_names)))
#return
goal_constraints = Constraints()
for i in range(len(joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=joint_names[i],
position=joint_values[i]))
self._goal.request.goal_constraints.append(goal_constraints)
def add_behavior(self, behavior_class, behavior_id):
"""
Adds another behavior as part of this behavior.
This other behavior should be declared as contained in the behavior manifest.
@type behavior_class: class
@param behavior_class: The class implementing the other behavior.
@type behavior_id: string
@param behavior_id: Unique identifier for this behavior instance.
"""
if not hasattr(self, 'contains'):
Logger.logerr(
'Behavior was not initialized! Please call superclass constructor.'
)
instance = behavior_class()
self.contains[behavior_id] = instance
def get_from_buffer(self, topic):
"""
Pops the oldest buffered message of the given topic.
@type topic: string
@param topic: The topic of interest.
"""
if not ProxySubscriberCached._topics[topic]['buffered']:
Logger.logwarn('Attempted to access buffer of non-buffered topic!')
return None
if len(ProxySubscriberCached._topics[topic]['msg_queue']) == 0:
return None
msg = ProxySubscriberCached._topics[topic]['msg_queue'][0]
ProxySubscriberCached._topics[topic][
'msg_queue'] = ProxySubscriberCached._topics[topic]['msg_queue'][
1:]
return msg
def _execute_single_state(self, state, force_exit=False):
result = None
try:
with UserData(reference=self._userdata,
remap=self._remappings[state.name],
input_keys=state.input_keys,
output_keys=state.output_keys) as userdata:
if force_exit:
state._entering = True
state.on_exit(userdata)
else:
result = state.execute(userdata)
except Exception as e:
result = None
self._last_exception = e
Logger.logerr('Failed to execute state %s:\n%s' %
(self.current_state_label, str(e)))
return result
def _execute_unlock(self, target):
if target == self.path or (self._locked and target == ''):
target = self.path
found_target = True
self._locked = False
else:
found_target = self._parent.unlock(target)
# provide feedback about unlock
self._pub.publish(
self._feedback_topic,
CommandFeedback(command="unlock",
args=[target, target if found_target else ""]))
if not found_target:
Logger.logwarn(
"Wanted to unlock %s, but could not find it in current path %s."
% (target, self.path))
else:
Logger.localinfo("--> Unlocking in state %s" % target)
def reset_joint_constraints(self, move_group=None):
'''
Reset the planning options to match the defaults
@param move_group : string or list specifying a particular move group(s) (default: None - change all relevant )
'''
flag = False
if move_group is None:
for group in self._move_group_list:
try:
ret = self.reset_joint_constraints(group)
if ret:
flag=ret
except:
pass
# Check to see that something was reset
if flag:
return flag
else:
raise Exception(" Failed to reset joint constraints for any relevant topic for move_group=(%s) !" % (str(move_group)))
elif bool(move_group) and all([isinstance(elem,basestring) for elem in move_group]):
# List of strings
for group in move_group:
try:
ret = self.reset_joint_constraints(group)
if ret:
flag=ret
except:
pass
# Check to see that something was reset
if flag:
return flag
else:
raise Exception(" Failed to reset joint constraints for any relevant topic for move_group=(%s) !" % (str(move_group)))
else:
# Base case to reset specified motion plan requests options
try:
# Reset the planning options for a given move group
ProxyMoveItClient._joint_constraints[move_group] = copy.deepcopy(ProxyMoveItClient._default_joint_constraints[move_group])
return True
except Exception as e:
Logger.logerr(" Invalid move group %s for joint constraints - not configured yet!" % (str(move_group)))
return False
def __init__(self, verbose=False):
if not ProxyTrajectoryClient._is_initialized:
ProxyTrajectoryClient._is_initialized = True
Logger.loginfo(
"Initializing proxy FollowJointTrajectory client...")
ProxyTrajectoryClient._client = ProxyActionClient({
ProxyTrajectoryClient._action_topic:
FollowJointTrajectoryAction
})
ProxyTrajectoryClient._verbose = verbose
# enable the IK Service
ik_ns = "ExternalTools/" + ProxyTrajectoryClient._limb + "/PositionKinematicsNode/IKService"
ProxyTrajectoryClient._iksvc = rospy.ServiceProxy(
ik_ns, SolvePositionIK)
rospy.wait_for_service(ik_ns, 5.0)
# enable the Baxter
ProxyTrajectoryClient._rs = baxter_interface.RobotEnable(
baxter_interface.CHECK_VERSION)
ProxyTrajectoryClient._init_state = ProxyTrajectoryClient._rs.state(
).enabled
print("Enabling robot... ")
ProxyTrajectoryClient._rs.enable()
# enable baxter limb interface
ProxyTrajectoryClient._baxter_limb_interface = baxter_interface.limb.Limb(
ProxyTrajectoryClient._limb)
# Get the names from joints
ProxyTrajectoryClient._joint_name = [
joint for joint in
ProxyTrajectoryClient._baxter_limb_interface.joint_names()
]
ProxyTrajectoryClient._gripper = baxter_interface.Gripper(
ProxyTrajectoryClient._limb)
self._goal = FollowJointTrajectoryGoal()
self._result = None
self._goal_time_tolerance = rospy.Time(0.1)
self._goal.goal_time_tolerance = self._goal_time_tolerance
def publish(self, topic, msg):
"""
Publishes a message on the specified topic.
@type topic: string
@param topic: The topic to publish on.
@type msg: message class (defined when created publisher)
@param msg: The message to publish.
"""
if topic not in ProxyPublisher._topics:
Logger.logwarn('ProxyPublisher: topic %s not yet registered!' %
topic)
return
try:
ProxyPublisher._topics[topic].publish(msg)
except Exception as e:
Logger.logwarn('Something went wrong when publishing to %s!\n%s' %
(topic, str(e)))
def use_behavior(self,
behavior_class,
behavior_id,
default_keys=None,
parameters=None):
"""
Creates a state machine implementing the given behavior to use it in the behavior state machine.
Behavior has to be added first.
@type behavior_class: class
@param behavior_class: The class implementing the other behavior.
@type behavior_id: string
@param behavior_id: Same identifier as used for adding.
@type default_keys: list
@param default_keys: List of input keys of the behavior which should be ignored
and instead use the default values as given by the behavior.
@type parameters: dict
@param parameters: Optional assignment of values to behavior parameters.
Any assigned parameter will be ignored for runtime customization,
i.e., cannot be overwritten by a user who runs the behavior.
"""
if behavior_id not in self.contains:
Logger.logerr('Tried to use a behavior without adding it!')
return None
if parameters is not None:
for parameter, value in parameters.items():
setattr(self.contains[behavior_id], parameter, value)
state_machine = self.contains[behavior_id]._get_state_machine()
if default_keys is not None:
state_machine._input_keys = list(
set(state_machine._input_keys) - set(default_keys))
for key in state_machine._input_keys:
state_machine._own_userdata(remove_key=key)
return state_machine
def setup_action_client(self, action_topic, action_type, wait_duration):
"""
Tries to set up a MoveIt MoveGroup action client for calling it later.
@param action_topic : string - The topic of the action to call.
@param action_type : string - class definition name for interface
@param wait_duration : Defines how long to wait for the given client if it is not available right now.
"""
if not isinstance(action_topic,str):
raise Exception(" ProxyMoveItClient - Invalid action topic %s " % ( action_topic ))
if action_topic not in ProxyMoveItClient._action_clients:
# We have not initialized this client yet
Logger.loginfo("Initializing proxy MoveIt client for "+action_topic+" ...")
try:
ProxyMoveItClient._action_clients[action_topic] = ProxyActionClient({action_topic: eval(action_type) }, wait_duration)
except Exception as e:
Logger.logwarn("ProxyMoveItClient setup error - %s"% (str(e)))
raise e
def _check_topic_available(self, topic, wait_duration=1):
"""
Checks whether a topic is available.
@type topic: string
@param topic: The topic of the action.
@type wait_duration: int
@param wait_duration: Defines how long to wait for the given client if it is not available right now.
"""
client = ProxyActionClient._clients.get(topic)
if client is None:
Logger.logerr(
"Action client %s not yet registered, need to add it first!" %
topic)
return False
t = Timer(.5, self._print_wait_warning, [topic])
t.start()
available = client.wait_for_server(
rospy.Duration.from_sec(wait_duration))
warning_sent = False
try:
t.cancel()
except Exception:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Action client %s timed out!" % topic)
return False
else:
if warning_sent:
Logger.loginfo("Finally found action client %s..." % (topic))
return True
def get_group_state(self, config_name, group_name, robot_name=None ):
'''
Get named configuration from semantic description
@param config_name string name of configuration
@param group_name string move group name that the configuration belongs to
@param robot_name string robot (default uses current or first robot in SRDF)
'''
config = None
try:
# Find the designated robot (or first in file)
if (robot_name is not None and robot_name != ProxyMoveItClient._robot_name):
for r in ProxyMoveItClient._srdf_ET.iter('robot'):
if robot_name == r.attrib['name']:
ProxyMoveItClient._robot_name = r.attrib['name']
ProxyMoveItClient._robot_semantics = r
Logger.loginfo(" Switching proxy to robot %s " % (ProxyMoveItClient._robot_name) )
if ProxyMoveItClient._robot_semantics is None:
Logger.logwarn('Did not find robot name in SRDF - %s' % robot_name)
return None
# Find the desired configuration
for c in ProxyMoveItClient._robot_semantics.iter('group_state'):
if (group_name is None or group_name == '' or group_name == c.attrib['group']) \
and c.attrib['name'] == config_name:
config = c
break
return config
except Exception as e:
Logger.logwarn('Unable to find named configuration in SRDF -\n%s' % str(e))
return None
def wait_for_any(self, topic, timeout=5.0):
"""
Blocks until there are any subscribers to the given topic.
@type topic: string
@param topic: The topic to publish on.
@type timeout: float
@param timeout: How many seconds should be the maximum blocked time.
"""
pub = ProxyPublisher._topics.get(topic)
if pub is None:
Logger.logerr(
"Publisher %s not yet registered, need to add it first!" %
topic)
return False
t = Timer(.5, self._print_wait_warning, [topic])
t.start()
available = self._wait_for_subscribers(pub, timeout)
warning_sent = False
try:
t.cancel()
except Exception:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Waiting for subscribers on %s timed out!" % topic)
return False
else:
if warning_sent:
Logger.loginfo("Finally found subscriber on %s..." % (topic))
return True
def _manually_transitionable_execute(self, *args, **kwargs):
if self._is_controlled and self._sub.has_buffered(
self._transition_topic):
command_msg = self._sub.get_from_buffer(self._transition_topic)
self._pub.publish(
self._feedback_topic,
CommandFeedback(command="transition",
args=[command_msg.target, self.name]))
if command_msg.target != self.name:
Logger.logwarn(
"Requested outcome for state %s but active state is %s" %
(command_msg.target, self.name))
else:
self._force_transition = True
outcome = self.outcomes[command_msg.outcome]
Logger.localinfo(
"--> Manually triggered outcome %s of state %s" %
(outcome, self.name))
return outcome
# otherwise, return the normal outcome
self._force_transition = False
return self.__execute(*args, **kwargs)
def setupService(self, topic, msg_type, persistent=False, wait_duration=10):
"""
Tries to set up a service caller for calling it later.
@type topic: string
@param topic: The topic of the service to call.
@type msg_type: service class
@param msg_type: The type of messages of this service.
@type persistent: bool
@param persistent: Defines if this service caller is persistent.
@type wait_duration: int
@param wait_duration: Defines how long to wait for the given service if it is not available right now.
"""
if topic not in ProxyServiceCaller._services:
warning_sent = False
available = False
try:
t = Timer(1, self._print_wait_warning, [topic])
t.start()
rospy.wait_for_service(topic, wait_duration)
available = True
except rospy.exceptions.ROSException, e:
available = False
try:
t.cancel()
except Exception as ve:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Service client %s timed out!" % topic)
else:
ProxyServiceCaller._services[topic] = rospy.ServiceProxy(topic, msg_type, persistent)
if warning_sent:
Logger.loginfo("Finally found action client %s..." % (topic))
def setupService(self, topic, msg_type, persistent=False, wait_duration=10):
"""
Tries to set up a service caller for calling it later.
@type topic: string
@param topic: The topic of the service to call.
@type msg_type: service class
@param msg_type: The type of messages of this service.
@type persistent: bool
@param persistent: Defines if this service caller is persistent.
@type wait_duration: int
@param wait_duration: Defines how long to wait for the given service if it is not available right now.
"""
if topic not in ProxyServiceCaller._services:
warning_sent = False
available = False
try:
t = Timer(1, self._print_warning, [topic])
t.start()
rospy.wait_for_service(topic, wait_duration)
available = True
except rospy.exceptions.ROSException, e:
available = False
try:
t.cancel()
except Exception as ve:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Service client %s timed out!" % topic)
else:
ProxyServiceCaller._services[topic] = rospy.ServiceProxy(topic, msg_type, persistent)
if warning_sent:
Logger.loginfo("Finally found action client %s..." % (topic))
def find_behavior(self, be_name):
"""
Searches for a behavior with the given name and returns it along with its ID.
@type be_name: string
@param be_name: Behavior ID to look up.
@return Tuple (be_id, be_entry) corresponding to the name or (None, None) if not found.
"""
find = lambda: next((id, be) for (id, be) # noqa: E731 (allow lambda, only used locally here)
in self._behavior_lib.items()
if be["name"] == be_name)
try:
return find()
except StopIteration:
Logger.logwarn("Did not find behavior '%s' in libary, updating..." % be_name)
self.parse_packages()
try:
return find()
except StopIteration:
Logger.logerr("Still cannot find behavior '%s' in libary after update, giving up!" % be_name)
return None, None
def get_sourcecode_filepath(self, be_id, add_tmp=False):
"""
Constructs a file path to the source code of corresponding to the given ID.
@type be_id: int
@param be_id: Behavior ID to look up.
@type add_tmp: bool
@param add_tmp: Append "_tmp" to the file to consider a temporary version.
@return String containing the absolute path to the source code file.
"""
be_entry = self.get_behavior(be_id)
if be_entry is None:
# rely on get_behavior to handle/log missing package
return None
try:
module_path = __import__(be_entry["package"]).__path__[-1]
except ImportError:
Logger.logwarn("Cannot import behavior package '%s', using 'rospack find' instead" % be_entry["package"])
# rp can find it because otherwise, the above entry would not exist
module_path = os.path.join(self._rp.get_path(be_entry["package"]), "src", be_entry["package"])
filename = be_entry["file"] + '.py' if not add_tmp else '_tmp.py'
return os.path.join(module_path, filename)
def get_goal_joint_constraints(self, move_group, joint_values, joint_names=None):
'''
Creates a JointConstraint for list of goal constraints, and creates a new goal based on list
of joint constraints for the current plan request specification
@param move_group : string specifying a particular move group
@param joint_values : list of joint values (must match the given joint names)
@param joint_names : list of string joint names (default: None uses all joints in group)
'''
if joint_names is None:
joint_names = ProxyMoveItClient._joint_names[move_group]
if len(joint_names) != len(joint_values):
Logger.logerr("Amount of given joint values (%d) does not match the amount of joints (%d) in group %s." % (len(joint_values), len(joint_names), move_group))
raise Exception("Amount of given joint values (%d) does not match the amount of joints (%d) in group %s." % (len(joint_values), len(joint_names), move_group))
joint_constraints = []
for i, name in enumerate(joint_names):
constraint = copy.deepcopy(ProxyMoveItClient._joint_constraints[move_group][name])
constraint.position = joint_values[i]
joint_constraints.append( constraint )
return joint_constraints
def _print_wait_warning(self, topic):
Logger.logwarn("Waiting for action client %s..." % (topic))
def _print_warning(self, topic):
Logger.logwarn("Waiting for service client %s..." % (topic))