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 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 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 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 __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 __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 __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 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 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 _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 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 __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))