def get_topic_type(self, topic, blocking=False):
"""
Get the topic type.
!!! Overriden from rostopic !!!
:param topic: topic name, ``str``
:param blocking: (default False) block until topic becomes available, ``bool``
:returns: topic type, real topic name and fn to evaluate the message instance
if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)``
:raises: :exc:`ROSTopicException` If master cannot be contacted
"""
topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic)
if topic_type:
return topic_type, real_topic, msg_eval
elif blocking:
sys.stderr.write(
"WARNING: topic [%s] does not appear to be published yet\n" %
topic)
while not rospy.is_shutdown():
topic_type, real_topic, msg_eval = rostopic._get_topic_type(
topic)
if topic_type:
return topic_type, real_topic, msg_eval
else:
rostopic._sleep(
10.
) # Change! Waiting for 10 seconds instead of 0.1 to reduce load
return None, None, None
def get_action_type(self, action_name):
try:
return rostopic._get_topic_type(
rospy.resolve_name(action_name) + '/goal')[0][:-4]
except TypeError:
raise JoyTeleopException(
"could not find action {}".format(action_name))
def main():
rospy.init_node('axclient', anonymous=True)
parser = OptionParser(__doc__.strip())
# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
# help="A testing flag")
# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
(options, args) = parser.parse_args(rospy.myargv())
if (len(args) != 2):
parser.error("You must specify the action topic name Eg: ./axclient.py my_action_topic")
# get action type from topic
topic_type = rostopic._get_topic_type("%s/goal"%args[1])[0]
# remove "Goal" string from action type
assert("Goal" in topic_type)
topic_type = topic_type[0:len(topic_type)-4]
action = DynamicAction(topic_type)
app = AXClientApp(action, args[1])
app.MainLoop()
app.OnQuit()
rospy.signal_shutdown('GUI shutdown')
def main():
rospy.init_node('axclient', anonymous=True)
parser = OptionParser(__doc__.strip())
# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
# help="A testing flag")
# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
(options, args) = parser.parse_args(rospy.myargv())
if (len(args) == 2):
# get action type via rostopic
topic_type = rostopic._get_topic_type("%s/goal" % args[1])[0]
# remove "Goal" string from action type
assert ("Goal" in topic_type)
topic_type = topic_type[0:len(topic_type) - 4]
elif (len(args) == 3):
topic_type = args[2]
print(topic_type)
assert ("Action" in topic_type)
else:
parser.error(
"You must specify the action topic name (and optionally type) Eg: ./axclient.py action_topic actionlib/TwoIntsAction "
)
action = DynamicAction(topic_type)
app = AXClientApp(action, args[1])
app.MainLoop()
app.OnQuit()
rospy.signal_shutdown('GUI shutdown')
def get_live_message_types():
"""
Gathers a list of all the types used in a running ROS system.
Return:
a list of types used in a running ROS system.
"""
print "Generating ROS Messages from rostopics"
master = rosgraph.Master('/rostopic')
rostopic_info = ""
try:
state = master.getSystemState()
pubs, subs, _ = state
topics_published_to = [item[0] for item in pubs]
topics_subscribed_to = [item[0] for item in subs]
unique_topics = list(set(topics_published_to + topics_subscribed_to))
types = []
for topic in unique_topics:
topic_type = rostopic._get_topic_type(topic)[0]
types.append(topic_type)
output_list = set(types)
print "Discovered %d types " % len(set(types))
return output_list
except socket.error:
raise ROSTopicIOException("Unable to communicate with master!")
def get_topic_type(self, topic, blocking=False):
"""
Get the topic type.
!!! Overriden from rostopic !!!
:param topic: topic name, ``str``
:param blocking: (default False) block until topic becomes available, ``bool``
:returns: topic type, real topic name and fn to evaluate the message instance
if the topic points to a field within a topic, e.g. /rosout/msg. fn is None otherwise. ``(str, str, fn)``
:raises: :exc:`ROSTopicException` If master cannot be contacted
"""
topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic)
if topic_type:
return topic_type, real_topic, msg_eval
elif blocking:
sys.stderr.write("WARNING: topic [%s] does not appear to be published yet\n"%topic)
while not rospy.is_shutdown():
topic_type, real_topic, msg_eval = rostopic._get_topic_type(topic)
if topic_type:
return topic_type, real_topic, msg_eval
else:
rostopic._sleep(10.) # Change! Waiting for 10 seconds instead of 0.1 to reduce load
return None, None, None
def get_action_type(self, action_name):
while not rospy.is_shutdown():
action_name = action_name[1:] if action_name[
0] == "/" else action_name
topic_type = rostopic._get_topic_type("/%s/goal" % action_name)[0]
# remove "Goal" string from action type
try:
assert ("Goal" in topic_type)
except TypeError:
rospy.logwarn(
"Topic %s doe not seem to be available yet, retrying in 1 second"
)
rospy.sleep(1.)
continue
else:
return roslib.message.get_message_class(topic_type[:-4])
def main():
rospy.init_node('axclient', anonymous=True)
parser = OptionParser(__doc__.strip())
# parser.add_option("-t","--test",action="store_true", dest="test",default=False,
# help="A testing flag")
# parser.add_option("-v","--var",action="store",type="string", dest="var",default="blah")
(options, args) = parser.parse_args(rospy.myargv())
if len(args) == 2:
# get action type via rostopic
action_name = args[1].rstrip('/')
topic_type = rostopic._get_topic_type("%s/goal" % action_name)[0]
if topic_type is None:
parser.error(
"unable to retrieve the topic type for goal topic '{0}/goal'\nAre you sure the action server for '{0}' is running?"
.format(action_name))
# remove "Goal" string from action type
if not topic_type.endswith("ActionGoal"):
parser.error("topic '%s/goal' is not an action goal topic" %
action_name)
action_type = topic_type[:-4]
elif len(args) == 3:
action_type = args[2]
print(action_type)
action_suffix = "Action"
if not action_type.endswith(action_suffix):
parser.error(
"the action type provided '%s' doesn't end with the '%s' suffix"
% (action_type, action_suffix))
else:
parser.error(
"You must specify the action topic name (and optionally type) Eg: ./axclient.py action_topic actionlib/TwoIntsAction "
)
action = DynamicAction(action_type)
app = AXClientApp(action, args[1])
app.MainLoop()
app.OnQuit()
rospy.signal_shutdown('GUI shutdown')
def get_action_type(self, action_name):
action_name = action_name[1:] if action_name[0] == "/" else action_name
topic_type = rostopic._get_topic_type("/%s/goal" % action_name)[0]
# remove "Goal" string from action type
assert ("Goal" in topic_type)
return roslib.message.get_message_class(topic_type[:-4])
def __init__(self, test):
self.test = test
#print "recorder_core: self.test.name:", self.test.name
recorder_config = self.load_data(
rospkg.RosPack().get_path("atf_recorder_plugins") +
"/config/recorder_plugins.yaml")
try:
# delete test_results directories and create new ones
if os.path.exists(self.test.generation_config["bagfile_output"]):
# shutil.rmtree(self.tests.generation_config"bagfile_output"]) #FIXME will fail if multiple test run concurrently
pass
else:
os.makedirs(self.test.generation_config["bagfile_output"])
except OSError:
pass
# lock for self variables of this class
self.lock = Lock()
# create bag file writer handle
self.lock_write = Lock()
self.bag = rosbag.Bag(
self.test.generation_config["bagfile_output"] + self.test.name +
".bag", 'w')
self.bag_file_writer = BagfileWriter(self.bag, self.lock_write)
# Init metric recorder
self.recorder_plugin_list = []
#print "recorder_config", recorder_config
if len(recorder_config) > 0:
for value in list(recorder_config.values()):
#print "value=", value
self.recorder_plugin_list.append(
getattr(atf_recorder_plugins, value)(self.lock_write,
self.bag_file_writer))
#print "self.recorder_plugin_list", self.recorder_plugin_list
#rospy.Service(self.topic + "recorder_command", RecorderCommand, self.command_callback)
self.diagnostics = None
rospy.Subscriber("/diagnostics_toplevel_state", DiagnosticStatus,
self.diagnostics_callback)
rospy.on_shutdown(self.shutdown)
# wait for topics, services, actions and tfs to become active
if test.robot_config != None and 'wait_for_topics' in test.robot_config:
for topic in test.robot_config["wait_for_topics"]:
rospy.loginfo("Waiting for topic '%s'...", topic)
rospy.wait_for_message(topic, rospy.AnyMsg)
rospy.loginfo("... got message on topic '%s'.", topic)
if test.robot_config != None and 'wait_for_services' in test.robot_config:
for service in test.robot_config["wait_for_services"]:
rospy.loginfo("Waiting for service '%s'...", service)
rospy.wait_for_service(service)
rospy.loginfo("... service '%s' available.", service)
if test.robot_config != None and 'wait_for_actions' in test.robot_config:
for action in test.robot_config["wait_for_actions"]:
rospy.loginfo("Waiting for action '%s'...", action)
# wait for action status topic
rospy.wait_for_message(action + "/status", rospy.AnyMsg)
# get action type of goal topic
topic_type = rostopic._get_topic_type(action + "/goal")[0]
# remove "Goal" string from action type
if topic_type == None or not "Goal" in topic_type: ## pylint: disable=unsupported-membership-test
msg = "Could not get type for action %s. type is %s" % (
action, topic_type)
rospy.logerr(msg)
raise ATFRecorderError(msg)
# remove "Goal" from type
topic_type = topic_type[0:len(topic_type) - 4] ## pylint: disable=unsubscriptable-object
client = SimpleActionClient(
action, roslib.message.get_message_class(topic_type))
# wait for action server
client.wait_for_server()
rospy.loginfo("... action '%s' available.", action)
if test.robot_config != None and 'wait_for_tfs' in test.robot_config:
listener = tf.TransformListener()
for root_frame, measured_frame in test.robot_config[
"wait_for_tfs"]:
rospy.loginfo(
"Waiting for transformation from '%s' to '%s' ...",
root_frame, measured_frame)
listener.waitForTransform(root_frame, measured_frame,
rospy.Time(), rospy.Duration(1))
rospy.loginfo(
"... transformation from '%s' to '%s' available.",
root_frame, measured_frame)
if test.robot_config != None and 'wait_for_diagnostics' in test.robot_config and test.robot_config[
"wait_for_diagnostics"]:
rospy.loginfo("Waiting for diagnostics to become OK ...")
r = rospy.Rate(100)
while not rospy.is_shutdown(
) and self.diagnostics != None and self.diagnostics.level != 0:
rospy.logdebug("... waiting for diagnostics to become OK ...")
r.sleep()
rospy.loginfo("... diagnostics are OK.")
self.active_topics = {}
# special case for tf: make sure tf_buffer is already filled (even before the testblocks are started). Thus we need to record /tf and /tf_static all the time, even if there is no active testblock using tf.
for testblock in self.test.testblocks:
topics = self.get_topics_of_testblock(testblock.name)
if "/tf" in topics or "/tf_static" in topics:
self.active_topics["/tf"] = ["always"]
self.active_topics["/tf_static"] = ["always"]
self.subscribers = {}
self.tf_static_message = TFMessage()
rospy.Timer(rospy.Duration(0.1), self.create_subscriber_callback)
rospy.Timer(rospy.Duration(0.1), self.tf_static_timer_callback)
rospy.loginfo("ATF recorder: started!")
def get_action_type(self, action_name):
try:
return rostopic._get_topic_type(rospy.resolve_name(action_name) + '/goal')[0][:-4]
except TypeError:
raise JoyTeleopException("could not find action {}".format(action_name))
def get_goal_type(self, action_name):
topic_type = rostopic._get_topic_type("/%s/goal" % action_name)[0]
# remove "Action" string from goal type
assert ("Action" in topic_type)
return roslib.message.get_message_class(topic_type[:-10] + "Goal")
