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 _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 _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 _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 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 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 _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 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 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 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 _check_service_available(self, topic, wait_duration=1):
"""
Checks whether a service is available.
@type topic: string
@param topic: The topic of the service.
@type wait_duration: int
@param wait_duration: Defines how long to wait for the given service if it is not available right now.
"""
client = ProxyServiceCaller._services.get(topic)
if client is None:
Logger.logerr(
"Service client %s not yet registered, need to add it first!" %
topic)
return False
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:
available = False
try:
t.cancel()
except Exception:
# already printed the warning
warning_sent = True
if not available:
Logger.logerr("Service client %s timed out!" % topic)
return False
else:
if warning_sent:
Logger.loginfo("Finally found service %s..." % (topic))
return True
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 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 load_semantic_description(self, semantic_description):
'''
Load semantic description from the parameter server for later retrieval
'''
if semantic_description is None:
Logger.logwarn(" No semantic description file loaded !")
return None
# Check existence of SRDF parameter.
# Values are read during runtime to allow modifications.
srdf_param = None
if rospy.has_param(semantic_description):
srdf_param = rospy.get_param(semantic_description)
else:
Logger.logwarn('Unable to get SRDF parameter - %s' % (semantic_description ))
return None
try:
ProxyMoveItClient._srdf_ET = ET.fromstring(srdf_param)
Logger.loginfo(" Loaded %s SRDF file " % (semantic_description))
except Exception as e:
Logger.logerr('Unable to parse given semantic description (SRDF) file - %s\n%s' % (semantic_description, str(e)))
return None
try:
for r in ProxyMoveItClient._srdf_ET.iter('robot'):
if ProxyMoveItClient._robot_name is None:
ProxyMoveItClient._robot_name = r.attrib['name']
ProxyMoveItClient._robot_semantics = r
Logger.loginfo(" Found robot %s in SRDF" % (ProxyMoveItClient._robot_name))
else:
Logger.loginfo(" additional robot %s in SRDF - not used at this time " % (r.attrib['name']))
except Exception as e:
Logger.logerr('Unable to parse given semantic description (SRDF) element tree for robot - %s\n%s' % (semantic_description, str(e)))
return None
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
class ProxyMoveItClient(object):
"""
A proxy for easily using MoveIt! standard capabilities via the RobotCommander interface
This code shares one RobotCommander interface among several action topics.
The proxy is used to handle parameterization of several MoveIt capabilities, and to initialize
various action interfaces. The actual interfaces to the capabilities are via individual
state implementations with Action interfaces.
"""
_robot_commander = None
_goal_defaults = None
_robot_name = None
_robot_semantics = None
_robot_description = None
_semantic_description = None
_action_clients = {} # Dictionary of Action clients per topic
_planning_scene = PlanningScene()
_planning_scene.robot_state.is_diff = True # Flags start state as empty to use current state on server
Logger.logerr(" ProxyMoveItClient: TODO - set up the default planning scene data and add handlers ")
# Plan request data structures used by several actions
_motion_plan_requests = {} # Dictionary of active plan requests per move group
_default_motion_plan_requests = {} # Dictionary of default plan requests per move group
_planning_options = {} # Dictionary of active planning options per move group
_default_planning_options = {} # Dictionary of default planning optionsper move group
_joint_constraints = {} # Dictionary of dictionaries per move group and joint name
_default_joint_constraints = {} # Dictionary of dictionaries per move group and joint name
_position_constraints = {} # Dictionary of position constraints per move group
_default_position_constraints = {} # Dictionary of position constraints per move group
_orientation_constraints = {} # Dictionary of orientation constraints per move group
_default_orientation_constraints = {} # Dictionary of orientation constraints per move group
_visibility_constraints = {} # Dictionary of visibility constraints per move group
_default_visibility_constraints = {} # Dictionary of visibility constraints per move group
_robot_states = {} # Dictionary of robot state queries per move group
_default_robot_states = {} # Dictionary of robot state queries per move group
_robot_start_states = {} # Dictionary of robot state queries per move group
_robot_target_states = {} # Dictionary of robot state queries per move group
_move_group_list = None # List of available move groups for this robot
_move_group_clients_={} # Dictionary of action topic names for each move group
_joint_names={} # Dictionary of joint name list dictionaries per move_group per topic
def __init__(self,
robot_description="/robot_description",
semantic_description=None,
move_group_capabilities="/move_group",
wait_duration=0.5):
"""
Creates a proxy for RobotCommander using a particular robot description.
@param robot_description : string name of robot description (default: "robot_description")
@param semantic_description : string name of robot semantic description (default: "None")
@param move_group_capabilities : string or dictionary of topics by group name
@param wait_duration : how long to wait for action server to become available
"""
if (robot_description is None):
#Logger.loginfo("ProxyMoveItClient - calling with robot_description is None " )
try:
if ProxyMoveItClient._robot_commander is None:
Logger.loginfo("ProxyMoveItClient - No robot commander is currently initialized!" )
else:
Logger.loginfo("ProxyMoveItClient - Using existing RobotCommander instance with %s" % (ProxyMoveItClient._robot_description))
except:
Logger.loginfo("ProxyMoveItClient global not initialized yet " )
elif (ProxyMoveItClient._robot_commander is None):
Logger.loginfo("Initializing robot data for proxy MoveIt client with %s ..." % robot_description)
moveit_commander.roscpp_initialize(sys.argv)
Logger.loginfo(" Loading robot commander for %s" % (robot_description))
ProxyMoveItClient._robot_commander = moveit_commander.RobotCommander(robot_description)
ProxyMoveItClient._robot_description = robot_description
# Load the semantic description as element tree to provide more direct access to data
if semantic_description is None:
semantic_description = robot_description+"_semantic"
Logger.loginfo(" Loading semantic description parameter %s" % (semantic_description))
ProxyMoveItClient._semantic_description = self.load_semantic_description(semantic_description)
# Get the active move groups for this set up
ProxyMoveItClient._move_group_list = ProxyMoveItClient._robot_commander.get_group_names()
Logger.loginfo(" Available move groups %s for this robot on proxy MoveIt client ..." % (str(ProxyMoveItClient._move_group_list)))
for group in ProxyMoveItClient._move_group_list:
joint_names = ProxyMoveItClient._robot_commander.get_joint_names(group)
ProxyMoveItClient._joint_names[group] = [name for name in joint_names]
Logger.loginfo(" %s : %s " % (group, str(ProxyMoveItClient._joint_names[group])))
# Set the default data for this move group
self.set_global_defaults(group)
try:
action_topic=None
if isinstance(move_group_capabilities, dict) and group in move_group_capabilities:
# Allow for a dictionary of action topics per move group
action_topic = move_group_capabilities[group]
else: # Should be a single string for default single move group capability
action_topic = move_group_capabilities
if not isinstance(action_topic,str):
raise Exception(" ProxyMoveItClient - Invalid action topic %s " % ( action_topic ))
if action_topic not in ProxyMoveItClient._action_clients:
# Defining a new MoveGroupAction client for this move group
Logger.loginfo("Initializing proxy MoveGroup client for "+action_topic+" ...")
ProxyMoveItClient._action_clients[action_topic] = ProxyActionClient({action_topic: eval("MoveGroupAction") }, wait_duration)
# Store the topic name of the client reference in per move group dictionary as well
ProxyMoveItClient._move_group_clients_[group] = action_topic
except Exception as e:
Logger.logwarn("ProxyMoveItClient MoveGroupAction client setup error - %s"% (str(e)))
raise e
else:
Logger.loginfo("Already initialized robot data for proxy MoveIt client - new call with %s ..." % robot_description)
if robot_description is not None and robot_description != ProxyMoveItClient._robot_description:
Logger.logerr(" Using existing Proxy with robot description %s vs. %s " % ( ProxyMoveItClient._robot_description, robot_description))
raise Exception(" ProxyMoveItClient - Requested Proxy with diffent robot description %s vs. %s - not supported!" % ( ProxyMoveItClient._robot_description, robot_description))
def load_semantic_description(self, semantic_description):
'''
Load semantic description from the parameter server for later retrieval
'''
if semantic_description is None:
Logger.logwarn(" No semantic description file loaded !")
return None
# Check existence of SRDF parameter.
# Values are read during runtime to allow modifications.
srdf_param = None
if rospy.has_param(semantic_description):
srdf_param = rospy.get_param(semantic_description)
else:
Logger.logwarn('Unable to get SRDF parameter - %s' % (semantic_description ))
return None
try:
ProxyMoveItClient._srdf_ET = ET.fromstring(srdf_param)
Logger.loginfo(" Loaded %s SRDF file " % (semantic_description))
except Exception as e:
Logger.logerr('Unable to parse given semantic description (SRDF) file - %s\n%s' % (semantic_description, str(e)))
return None
try:
for r in ProxyMoveItClient._srdf_ET.iter('robot'):
if ProxyMoveItClient._robot_name is None:
ProxyMoveItClient._robot_name = r.attrib['name']
ProxyMoveItClient._robot_semantics = r
Logger.loginfo(" Found robot %s in SRDF" % (ProxyMoveItClient._robot_name))
else:
Logger.loginfo(" additional robot %s in SRDF - not used at this time " % (r.attrib['name']))
except Exception as e:
Logger.logerr('Unable to parse given semantic description (SRDF) element tree for robot - %s\n%s' % (semantic_description, str(e)))
return None
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 set_global_defaults( self, group ):
"""
Initializes the proxy with default parameters to use with connections
@param group : string move group name
"""
# Default planning parameters for new connections
num_planning_attempts=5
allowed_planning_time=10.0
plan_only=True
planner_id='RRTConnectkConfigDefault'
# Set the default plan request data
plan_request = MotionPlanRequest()
plan_request.start_state.is_diff = True # Flags start state as empty to use current state on server
plan_request.num_planning_attempts = num_planning_attempts
plan_request.allowed_planning_time = allowed_planning_time
plan_request.planner_id = planner_id
plan_request.group_name = group
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._motion_plan_requests[group] = plan_request
ProxyMoveItClient._default_motion_plan_requests[group] = copy.deepcopy(plan_request)
planning_options = PlanningOptions()
planning_options.plan_only = plan_only
planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._planning_options[group] = planning_options
ProxyMoveItClient._default_planning_options[group] = copy.deepcopy(planning_options)
# Set up the default joint constraints
joint_constraints = {}
for name in ProxyMoveItClient._joint_names[group]:
joint_constraints[name] = JointConstraint(joint_name=name, tolerance_above=0.05, tolerance_below=0.05,weight=1.0)
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._joint_constraints[group] = joint_constraints
ProxyMoveItClient._default_joint_constraints[group] = copy.deepcopy(joint_constraints)
# @TODO - add real constraints
ProxyMoveItClient._position_constraints[group] = []
ProxyMoveItClient._default_position_constraints[group] = []
ProxyMoveItClient._orientation_constraints[group] = []
ProxyMoveItClient._default_orientation_constraints[group] = []
ProxyMoveItClient._visibility_constraints[group] = []
ProxyMoveItClient._default_visibility_constraints[group] = []
ProxyMoveItClient._robot_states[group] = RobotState()
ProxyMoveItClient._default_robot_states[group] = RobotState()
def setup_multiple_action_clients(self, action_topics, wait_duration=2.0):
"""
Tries to set up a MoveIt MoveGroup action client for calling it later.
@param action_topics : list of tuples of Action type and topic names
@param wait_duration: Defines how long to wait for the given client if it is not available right now.
"""
if action_topics is not None:
print "ProxyMoveItClient: setup_multiple_action_clients ..."
for action_type, topics in action_topics:
print "Action type: ",action_type," : ",topics
for topic in topics:
self.setup_action_client(topic, action_type, wait_duration)
else:
print "ProxyMoveItClient: no additional action clients"
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 dump_moveit_client(self):
'''
This dumps the ProxyMoveItClient data for debugging and status
'''
data = "\n*******************************\n"
data += "------ ProxyMoveItClient ------\n"
data += " Robot: "+ProxyMoveItClient._robot_description+"\n"
data += " --- Action Clients ---\n"
for topic in ProxyMoveItClient._action_clients:
data += " client: "+topic+"\n"
data += "----------------------------\n"
for group in ProxyMoveItClient._move_group_list:
data += str(self.dump_moveit_goal(group))
data += "----------------------------\n\n"
data += "--------------------------------\n"
data += "################################\n"
return data
def dump_moveit_goal(self, group):
'''
This dumps the current MoveGroupGoal data for debugging and status
@param group : string for a specific move group or
'''
data = " group: "+group+"\n"
data += " --- MotionPlanRequest ---\n"
data += " "+str(ProxyMoveItClient._motion_plan_requests[group])
data += "\n ----------------------------\n"
data += " --- PlanningOption ---\n"
data += " "+str(ProxyMoveItClient._planning_options[group])
data += "\n ----------------------------\n"
return data
def get_error_msg(self, error_code):
'''
Returns error message string based on
@param error_code : code returned from MoveIt!
'''
for key, value in MoveItErrorCodes.__dict__.items():
if value == error_code.val and key[0] != "_":
return key + (" (%s)" % str(error_code.val))
return "unknown error (%s)" % str(error_code.val)
def is_available(self, action_topic):
"""
Checks if the client on the given action topic is available.
@param topic: string The topic of interest.
"""
if(not action_topic in ProxyMoveItClient._action_clients):
raise Exception("ProxyMoveItClient - topic %s is not initialized yet!" % (action_topic))
return ProxyMoveItClient._action_clients[action_topic].is_available(action_topic)
def get_state(self, action_topic):
"""
Returns current state of the ActionLib client.
@param topic: string The topic of interest.
"""
if(not action_topic in ProxyMoveItClient._action_clients):
raise Exception("ProxyMoveItClient - topic %s is not initialized yet!" % (action_topic))
return ProxyMoveItClient._action_clients[action_topic].get_state(action_topic)
def has_result(self, action_topic):
"""
Checks if the client on the given action topic has an active result.
@param topic: string The topic of interest.
"""
if(not action_topic in ProxyMoveItClient._action_clients):
raise Exception("ProxyMoveItClient - topic %s is not initialized yet!" % (action_topic))
return ProxyMoveItClient._action_clients[action_topic].has_result(action_topic)
def get_result(self, action_topic):
"""
Gets that latest result from the given action topic.
@param topic: string The topic of interest.
"""
if(not action_topic in ProxyMoveItClient._action_clients):
raise Exception("ProxyMoveItClient - topic %s is not initialized yet!" % (action_topic))
return ProxyMoveItClient._action_clients[action_topic].get_result(action_topic)
def cancel(self, action_topic):
"""
Cancel any active goals on the given action topic
@param topic: string The topic of interest.
"""
if(not action_topic in ProxyMoveItClient._action_clients):
raise Exception("ProxyMoveItClient - topic %s is not initialized yet!" % (action_topic))
return ProxyMoveItClient._action_clients[action_topic].cancel(action_topic)
def connect_action_server(self, action_topic, action_type, wait_duration=0.0):
"""
Checks if the client on the given action topic is available.
@param action_topic: string The topic of interest.
@param action_type: string The interface type
@param wait_duration : float How long to wait for a connection
"""
if not action_topic in ProxyMoveItClient._action_clients:
raise Exception("ProxyMoveItClient - topic %s is not initialized yet!" % (action_topic))
ProxyMoveItClient._action_clients[action_topic].setupClient(action_topic, eval(action_type) , wait_duration)
return ProxyMoveItClient._action_clients[action_topic].is_available(action_topic)
def is_active(self, action_topic):
"""
Determines if an action request is already being processed on the given topic.
@type topic: string
@param topic: The topic of interest.
"""
if not action_topic in ProxyMoveItClient._action_clients:
raise Exception("ProxyMoveItClient - is_active topic %s is not initialized yet!" % (action_topic))
return ProxyMoveItClient._action_clients[action_topic].is_active(action_topic)
def reset_motion_plan_request(self, move_group=None):
'''
Clears the specified dictionary for move group
@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_motion_plan_request(group)
if ret:
flag=ret
except:
pass
# Check to see that something was reset
if flag:
return flag
else:
raise Exception(" Failed to reset dictionary 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_motion_plan_request(group)
if ret:
flag=ret
except:
pass
# Check to see that something was reset
if flag:
return flag
else:
raise Exception(" Failed to reset dictionary 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._motion_plan_requests[move_group] = copy.deepcopy(ProxyMoveItClient._default_motion_plan_requests[move_group])
return True
except Exception as e:
Logger.logerr(" Invalid move group %s for motion plan request - not configured yet!" % (str(move_group)))
return False
def reset_planning_options(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_planning_options(group)
if ret:
flag=ret
except:
pass
# Check to see that something was reset
if flag:
return flag
else:
raise Exception(" Failed to reset planning options 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_planning_options(group)
if ret:
flag=ret
except:
pass
# Check to see that something was reset
if flag:
return flag
else:
raise Exception(" Failed to reset planning options 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._planning_options[move_group] = copy.deepcopy(ProxyMoveItClient._default_planning_options[move_group])
return True
except Exception as e:
Logger.logerr(" Invalid move group %s for planning options - not configured yet!" % (str(move_group)))
return False
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 reset_position_constraints(self, move_group):
'''
Reset the current position constraints to the defaults
@param move_group : string specifying a particular move group
'''
#@TODO - handle reset all as with joints
ProxyMoveItClient._position_constraints[move_group] = copy.deepcopy(ProxyMoveItClient._default_position_constraints[move_group])
def reset_orientation_constraints(self, move_group):
'''
Reset the current orientation constraints to the defaults
@param move_group : string specifying a particular move group
'''
#@TODO - handle reset all as with joints
ProxyMoveItClient._orientation_constraints[move_group] = copy.deepcopy(ProxyMoveItClient._default_orientation_constraints[move_group])
def reset_visibility_constraints(self, move_group):
'''
Reset the current orientation constraints to the defaults
@param move_group : string specifying a particular move group
'''
#@TODO - handle reset all as with joints
ProxyMoveItClient._visibility_constraints[move_group] = copy.deepcopy(ProxyMoveItClient._default_visibility_constraints[move_group])
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 set_position_constraints(self, move_group, position_constraints):
'''
Update the current position constraints for a given move group
@param move_group : string specifying a particular move group
@param position_constraints : list of position constraints
'''
ProxyMoveItClient._position_constraints[move_group] = position_constraints
def set_orientation_constraints(self, move_group, orientation_constraints):
'''
Update the current position constraints for a given move group
@param move_group : string specifying a particular move group
@param orientation_constraints : list of orientation constraints
'''
ProxyMoveItClient._orientation_constraints[move_group] = orientation_constraints
def set_visibility_constraints(self, move_group, visibility_constraints):
'''
Update the current position constraints for a given move group
@param move_group : string specifying a particular move group
@param visibility_constraints : list of visibility constraints
'''
ProxyMoveItClient._visibility_constraints[move_group] = visibility_constraints
def get_joint_constraints(self, move_group):
'''
Retrieve the current joint constraints as a list for a given move group
@param move_group : string specifying a particular move group
'''
return copy.deepcopy(ProxyMoveItClient._joint_constraints[move_group].values())
def get_position_constraints(self, move_group):
'''
Retrieve the current position constraints for a given move group
@param move_group : string specifying a particular move group
'''
return copy.deepcopy(ProxyMoveItClient._position_constraints[move_group])
def get_orientation_constraints(self, move_group):
'''
Retrieve the current orientation constraints for a given move group
@param move_group : string specifying a particular move group
'''
return copy.deepcopy(ProxyMoveItClient._orientation_constraints[move_group])
def get_visibility_constraints(self, move_group):
'''
Retrieve the current visibility constraints for a given move group
@param move_group : string specifying a particular move group
'''
return copy.deepcopy(ProxyMoveItClient._visibility_constraints[move_group])
def get_constraints(self, move_group):
'''
Creates a Constraints structure for the specified move_group based on
current stored information.
@param move_group : string specifying a particular move group
'''
constraints = Constraints()
constraints.joint_constraints = self.get_joint_constraints(move_group)
constraints.position_constraints = self.get_position_constraints(move_group)
constraints.orientation_constraints = self.get_orientation_constraints(move_group)
constraints.visibility_constraints = self.get_visibility_constraints(move_group)
return constraints
def set_robot_state(self, move_group, robot_state):
#@TODO - fix this
Logger.logerr("ProxyMoveItClient: set_robot_state not finished!")
pass
def get_robot_state(self, move_group):
'''
Retrieve the current robot state structure for this move group
@param move_group : string specifying a particular move group
'''
return copy.deepcopy(ProxyMoveItClient._robot_states[move_group])
def set_robot_start_state(self, move_group, robot_state):
'''
Store the robot state used at start of trajectory for this move group
@param move_group : string specifying a particular move group
@param robot_state : robot state data structure
'''
ProxyMoveItClient._robot_start_states[move_group] = robot_state
def get_robot_start_state(self, move_group):
'''
Retrieve the current robot state structure for this move group
@param move_group : string specifying a particular move group
'''
return copy.deepcopy(ProxyMoveItClient._robot_start_states[move_group])
def send_move_group_joint_space_goal(self, move_group, joint_names, joint_values, plan_only=None):
'''
Retrieve the current move group action configuration and send the move request
@param move_group : string specifying a particular move group
@param joint_names : list of joint names for command
@param joint_values : list of joint values for target
'''
# We should have already set this pairing up during initialization
action_topic = ProxyMoveItClient._move_group_clients_[move_group]
# Get the latest setup of of the MoveGroup goal data
goal = MoveGroupGoal()
goal.request = copy.deepcopy(ProxyMoveItClient._motion_plan_requests[move_group])
goal.planning_options = copy.deepcopy(ProxyMoveItClient._planning_options[move_group])
if (plan_only is not None):
goal.planning_options.plan_only = plan_only
goal_constraints = Constraints()
for i in range(len(joint_names)):
jc = ProxyMoveItClient._joint_constraints[move_group][joint_names[i]]
goal_constraints.joint_constraints.append(JointConstraint(
joint_name=joint_names[i],
position=joint_values[i],
tolerance_above=jc.tolerance_above,
tolerance_below=jc.tolerance_above,
weight=jc.weight))
goal.request.goal_constraints.append(goal_constraints)
ProxyMoveItClient._action_clients[action_topic].send_goal(action_topic, goal)
def __init__(self,
robot_description="/robot_description",
semantic_description=None,
move_group_capabilities="/move_group",
wait_duration=0.5):
"""
Creates a proxy for RobotCommander using a particular robot description.
@param robot_description : string name of robot description (default: "robot_description")
@param semantic_description : string name of robot semantic description (default: "None")
@param move_group_capabilities : string or dictionary of topics by group name
@param wait_duration : how long to wait for action server to become available
"""
if (robot_description is None):
#Logger.loginfo("ProxyMoveItClient - calling with robot_description is None " )
try:
if ProxyMoveItClient._robot_commander is None:
Logger.loginfo("ProxyMoveItClient - No robot commander is currently initialized!" )
else:
Logger.loginfo("ProxyMoveItClient - Using existing RobotCommander instance with %s" % (ProxyMoveItClient._robot_description))
except:
Logger.loginfo("ProxyMoveItClient global not initialized yet " )
elif (ProxyMoveItClient._robot_commander is None):
Logger.loginfo("Initializing robot data for proxy MoveIt client with %s ..." % robot_description)
moveit_commander.roscpp_initialize(sys.argv)
Logger.loginfo(" Loading robot commander for %s" % (robot_description))
ProxyMoveItClient._robot_commander = moveit_commander.RobotCommander(robot_description)
ProxyMoveItClient._robot_description = robot_description
# Load the semantic description as element tree to provide more direct access to data
if semantic_description is None:
semantic_description = robot_description+"_semantic"
Logger.loginfo(" Loading semantic description parameter %s" % (semantic_description))
ProxyMoveItClient._semantic_description = self.load_semantic_description(semantic_description)
# Get the active move groups for this set up
ProxyMoveItClient._move_group_list = ProxyMoveItClient._robot_commander.get_group_names()
Logger.loginfo(" Available move groups %s for this robot on proxy MoveIt client ..." % (str(ProxyMoveItClient._move_group_list)))
for group in ProxyMoveItClient._move_group_list:
joint_names = ProxyMoveItClient._robot_commander.get_joint_names(group)
ProxyMoveItClient._joint_names[group] = [name for name in joint_names]
Logger.loginfo(" %s : %s " % (group, str(ProxyMoveItClient._joint_names[group])))
# Set the default data for this move group
self.set_global_defaults(group)
try:
action_topic=None
if isinstance(move_group_capabilities, dict) and group in move_group_capabilities:
# Allow for a dictionary of action topics per move group
action_topic = move_group_capabilities[group]
else: # Should be a single string for default single move group capability
action_topic = move_group_capabilities
if not isinstance(action_topic,str):
raise Exception(" ProxyMoveItClient - Invalid action topic %s " % ( action_topic ))
if action_topic not in ProxyMoveItClient._action_clients:
# Defining a new MoveGroupAction client for this move group
Logger.loginfo("Initializing proxy MoveGroup client for "+action_topic+" ...")
ProxyMoveItClient._action_clients[action_topic] = ProxyActionClient({action_topic: eval("MoveGroupAction") }, wait_duration)
# Store the topic name of the client reference in per move group dictionary as well
ProxyMoveItClient._move_group_clients_[group] = action_topic
except Exception as e:
Logger.logwarn("ProxyMoveItClient MoveGroupAction client setup error - %s"% (str(e)))
raise e
else:
Logger.loginfo("Already initialized robot data for proxy MoveIt client - new call with %s ..." % robot_description)
if robot_description is not None and robot_description != ProxyMoveItClient._robot_description:
Logger.logerr(" Using existing Proxy with robot description %s vs. %s " % ( ProxyMoveItClient._robot_description, robot_description))
raise Exception(" ProxyMoveItClient - Requested Proxy with diffent robot description %s vs. %s - not supported!" % ( ProxyMoveItClient._robot_description, robot_description))
def set_robot_state(self, move_group, robot_state):
#@TODO - fix this
Logger.logerr("ProxyMoveItClient: set_robot_state not finished!")
pass
