def generate_type_info_msg(self):
"""
Basic connection details are provided by get system state from the master, which is
a one shot call to give you information about every connection possible. it does
not however provide type info information and the only way of retrieving that from
the master is making one xmlrpc call to the master for every single connection.
This gets expensive, so generating this information is usually delayed until we
need it and done via this method.
"""
if self.type_info is None:
if self.type == PUBLISHER or self.type == SUBSCRIBER:
try:
self.type_info = rostopic.get_topic_type(
self.name)[0] # message type
self.type_msg = self.type_info
except rostopic.ROSTopicIOException as topic_exc:
rospy.logwarn(topic_exc)
elif self.type == SERVICE:
try:
self.type_info = rosservice.get_service_uri(self.name)
self.type_msg = rosservice.get_service_type(self.name)
except rosservice.ROSServiceIOException as service_exc:
rospy.logwarn(service_exc)
elif self.type == ACTION_SERVER or self.type == ACTION_CLIENT:
try:
goal_topic = self.name + '/goal'
goal_topic_type = rostopic.get_topic_type(goal_topic)
self.type_info = re.sub(
'ActionGoal$', '',
goal_topic_type[0]) # Base type for action
self.type_msg = self.type_info
except rostopic.ROSTopicIOException as topic_exc:
rospy.logwarn(topic_exc.msg)
return self # chaining
def add_trigger(topic, expr, select, index, wait=False):
global topics
if topic in topics:
topic_type = topics[topic]
topic_class = roslib.message.get_message_class(topic_type)
else:
topic_type, _, _ = rostopic.get_topic_type(topic)
topic_class, _, _ = rostopic.get_topic_class(topic)
if topic_type is None:
if wait is False:
rospy.loginfo('%s is not published yet', topic)
return None
elif wait is True:
rate = rospy.Rate(1)
while not rospy.is_shutdown() and topic_type is None:
topic_type, _, _ = rostopic.get_topic_type(topic)
topic_class, _, _ = rostopic.get_topic_class(topic)
rospy.loginfo('waiting topic %s' % topic)
rate.sleep()
else:
raise ValueError('wait should be bool')
if (topic_class is None):
rospy.loginfo('%s is not builded yet', topic_type)
return None
try:
cb_obj = gen_callback(expr, select, index)
rospy.loginfo("start subscribe (topic=%s type=%s)", topic, topic_type)
sub = rospy.Subscriber(topic, topic_class, cb_obj)
except Exception, e:
rospy.loginfo(str(e))
return None
def __init__(self, sub="/bvt_P900/RT/image"):
self.sub = sub
self.image_sub = rospy.Subscriber(sub, Image, self.image_callback)
self.bridge = CvBridge()
self.previous_features = None
self.current_features = None
data_type = rostopic.get_topic_type('/nav/nav_sts', blocking=False)[0]
data_class = roslib.message.get_message_class(data_type)
self.nav_sub = rospy.Subscriber('/nav/nav_sts', data_class,
self.nav_callback)
self.nav_y = None
self.nav_x = None
self.nav_yaw = None
data_type = rostopic.get_topic_type('/bvt_P900/RT/parameter',
blocking=False)[0]
data_class = roslib.message.get_message_class(data_type)
self.nav_sub = rospy.Subscriber('/bvt_P900/RT/parameter', data_class,
self.parameter_callback)
self.max_fov = None
self.height = None
self.width = None
self.origin_row = None
self.origin_col = None
self.max_range = None
self.pcl_pub = rospy.Publisher("/my_pcl_topic", PointCloud)
self.image_pub = rospy.Publisher(str(sub) + "_marked", Image)
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._start_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(
self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to previously unavailable topic %s'
% self._control_manager_diagnostics_topic)
else:
Logger.logwarn(
'[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.'
% self._control_manager_diagnostics_topic)
if not self._connected_odom:
(msg_path_odom, msg_topic_odom,
fn_odom) = rostopic.get_topic_type(self._odometry_topic)
if msg_topic_odom == self._odometry_topic:
msg_type_odom = self._get_msg_from_path(msg_path_odom)
self._sub_odom = ProxySubscriberCached(
{self._odometry_topic: msg_type_odom})
self._connected_odom = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._odometry_topic)
else:
Logger.logwarn(
'[ServiceRotateRelativeToGoToState]: Topic %s still not available, giving up.'
% self._odometry_topic)
if self._connected and self._sub_cont_diag.has_msg(
self._control_manager_diagnostics_topic):
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s'
% self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(
self._control_manager_diagnostics_topic)
if self._connected_odom and self._sub_odom.has_msg(
self._odometry_topic):
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Waiting for msg from topic %s'
% self._odometry_topic)
self._sub_odom.remove_last_msg(self._odometry_topic)
self._failed = False
def get_goal_type(action_name):
try:
type, name, fn = rostopic.get_topic_type("{}/goal".format(action_name))
return type
except rospy.ROSException as e:
try:
type, name, fn = rostopic.get_topic_type("{}/Goal".format(action_name), e)
return type
except rospy.ROSException as e2:
rospy.logerr("Couldnt find feedback type for action {}".format(action_name), e2)
return None
def get_goal_type(action_name):
try:
type, name, fn = rostopic.get_topic_type(action_name + "/goal")
return type
except rospy.ROSException as e:
try:
type, name, fn = rostopic.get_topic_type(action_name + "/Goal", e)
return type
except rospy.ROSException as e2:
rospy.logerr(
"Couldnt find feedback type for action " + action_name, e2)
return None
def generate_advertisement_connection_details(self, connection_type, name,
node):
'''
Creates all the extra details to create a connection object from an
advertisement rule. This is a bit different to the previous one - we just need
the type and single node uri that everything originates from (don't need to generate all
the pub/sub connections themselves.
Probably flips could be merged into this sometime, but it'd be a bit gnarly.
@param connection_type : the connection type (one of gateway_msgs.msg.ConnectionType)
@type string
@param name : the name of the connection
@type string
@param node : the master node name it comes from
@param string
@return the utils.Connection object complete with type_info and xmlrpc_uri
@type utils.Connection
'''
# Very important here to check for the results of xmlrpc_uri and especially topic_type
# https://github.com/robotics-in-concert/rocon_multimaster/issues/173
# In the watcher thread, we get the local connection index (whereby the arguments of this function
# come from) via master.get_connection_state. That means there is a small amount of time from
# getting the topic name, to checking for hte xmlrpc_uri and especially topic_type here in which
# the topic could have disappeared. When this happens, it returns None.
connection = None
xmlrpc_uri = self.lookupNode(node)
if xmlrpc_uri is None:
return connection
if connection_type == rocon_python_comms.PUBLISHER or connection_type == rocon_python_comms.SUBSCRIBER:
type_info = rostopic.get_topic_type(name)[0] # message type
if type_info is not None:
connection = utils.Connection(
gateway_msgs.Rule(connection_type, name, node), type_info,
type_info, xmlrpc_uri)
elif connection_type == rocon_python_comms.SERVICE:
type_info = rosservice.get_service_uri(name)
type_msg = rosservice.get_service_type(name)
if type_info is not None:
connection = utils.Connection(
gateway_msgs.Rule(connection_type, name, node), type_msg,
type_info, xmlrpc_uri)
elif connection_type == rocon_python_comms.ACTION_SERVER or connection_type == rocon_python_comms.ACTION_CLIENT:
goal_topic = name + '/goal'
goal_topic_type = rostopic.get_topic_type(goal_topic)
type_info = re.sub('ActionGoal$', '',
goal_topic_type[0]) # Base type for action
if type_info is not None:
connection = utils.Connection(
gateway_msgs.Rule(connection_type, name, node), type_info,
type_info, xmlrpc_uri)
return connection
def topic_wait_type(self, topic_name, retries=5, retrysleep=1):
resolved_topic_name = rospy.resolve_name(topic_name)
topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name)
retry = 0
while not topic_type and retry < 5:
print 'Topic {topic} not found. Retrying...'.format(
topic=resolved_topic_name)
rospy.rostime.wallsleep(retrysleep)
retry += 1
topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name)
if retry >= retries:
self.fail(
"Topic {0} not found ! Failing.".format(resolved_topic_name))
return topic_type
def test_get_topic_type(self):
import rostopic
self.assertEquals((None, None, None), rostopic.get_topic_type('/fake', blocking=False))
t, n, f = rostopic.get_topic_type('/rosout', blocking=False)
self.assertEquals('rosgraph_msgs/Log', t)
self.assertEquals('/rosout', n)
self.assert_(f is None)
t, n, f = rostopic.get_topic_type('/rosout/name', blocking=False)
self.assertEquals('rosgraph_msgs/Log', t)
self.assertEquals('/rosout', n)
self.failIf(f is None)
from rosgraph_msgs.msg import Log
self.assertEquals("bob", f(Log(name="bob")))
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self.ref_frame = userdata.ref_frame
self._topic = userdata.camera_topic
self._camera_frame = userdata.camera_frame
self._connected = False
self._failed = False
self._start_time = rospy.get_rostime()
# Subscribe to the topic for the logical camera
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn(
'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...'
% (self._topic, self.name, str(msg_topic)))
# Get transform between camera and robot_base
try:
self._transform = self._tf_buffer.lookup_transform(
self.ref_frame, self._camera_frame, rospy.Time(0),
rospy.Duration(1.0))
except Exception as e:
Logger.logwarn('Could not transform pose: ' + str(e))
self._failed = True
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
for i in [0, 1, 2, 3, 4, 5]:
self._topic = "/ariac/logical_camera_" + str(i)
self._start_time = rospy.Time.now()
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
elapsed = rospy.get_rostime() - self._start_time
while (elapsed.to_sec() < self._wait):
elapsed = rospy.get_rostime() - self._start_time
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for model in message.models:
if model.type == userdata.part_type:
userdata.bin = "bingr" + str(i) + "PreGrasp"
userdata.camera_topic = "/ariac/logical_camera_" + str(
i)
userdata.camera_frame = "logical_camera_" + str(
i) + "_frame"
Logger.loginfo("bingr" + str(i) + "PreGrasp")
userdata.ref_frame = "torso_main"
return 'continue'
Logger.loginfo("part_type not found")
return 'failed'
def wait_topic_ready(topic_name, url):
remote_topic_type = ""
while remote_topic_type == "":
remote_topic_type = get_remote_topic_type(topic_name, url)
#print remote_topic_type+" remote_topic_type"
if (remote_topic_type == ""):
rospy.loginfo(
"Failed to get the remote type of topic %s. Retrying...",
topic_name)
time.sleep(1)
local_topic_type = (None, None, None)
while local_topic_type[0] == None:
local_topic_type = get_topic_type(topic_name)
#print str(local_topic_type)+" local_topic_type"
if (local_topic_type[0] == None):
rospy.loginfo(
"Failed to get the local type of topic %s. Retrying...",
topic_name)
time.sleep(1)
if remote_topic_type == local_topic_type[0]:
#print str(local_topic_type)+" equal"
return local_topic_type[0]
else:
return None
def __init__(self):
self.is_ready = None
self.pub = rospy.Publisher('notification', Notification)
rospy.init_node('sensor_notifier_node')
sensor_topic = rospy.get_param('~sensor_topic', None)
if sensor_topic == None:
rospy.logerr('param sensor_topic not defined')
self.notification_text = rospy.get_param('~notification_text', u'sensor activated')
self.notification_level = rospy.get_param('~notification_level', 2)
self.notification_target = rospy.get_param('~notification_target', 'voice,log')
self.notification_value = rospy.get_param('~notification_value', '1')
self.criteria = rospy.get_param('~criteria', 'eq')
self.interval = rospy.get_param('~interval', 30)
self.notif_time = rospy.Time()
# subscribe to sensor topic
data_type = rostopic.get_topic_type(sensor_topic, blocking=False)[0]
if data_type:
data_class = roslib.message.get_message_class(data_type)
rospy.Subscriber(sensor_topic, data_class, self.callback)
rospy.loginfo("start listening to %s, notification: %s %s", sensor_topic, self.criteria, str(self.notification_value))
self.is_ready = 1
else:
rospy.logerr("error getting type for topic " + sensor_topic)
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
# One of the outcomes declared above.
for i in [1,2,3,4,5,6]:
self._topic = "/ariac/logical_camera_stock"+str(i)
self._start_time = rospy.Time.now()
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
elapsed = rospy.get_rostime() - self._start_time;
while (elapsed.to_sec() < self._wait):
elapsed = rospy.get_rostime() - self._start_time;
if self._sub.has_msg(self._topic):
message = self._sub.get_last_msg(self._topic)
for model in message.models:
if model.type == userdata.part_type:
if userdata.arm_id == "Right_Arm":
userdata.gantry_pos = "Gantry_Part"+str(i)+"_R"
elif userdata.arm_id == "Left_Arm":
userdata.gantry_pos = "Gantry_Part"+str(i)+"_L"
userdata.camera_topic = "/ariac/logical_camera_stock"+str(i)
userdata.camera_frame = "logical_camera_stock"+str(i)+"_frame"
Logger.loginfo("Gantry_Part"+str(i))
if i < 5:
return 'bin'
else:
return 'shelf'
Logger.loginfo("part_type not found")
return 'not_found'
def __init__(self, service_topic, control_manager_diagnostics_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceGoToState,
self).__init__(input_keys=['goal'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._service_topic = rospy.resolve_name(service_topic)
self._control_manager_diagnostics_topic = rospy.resolve_name(
control_manager_diagnostics_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._service_topic: mrs_msgs.srv.Vec4})
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceGoToState]: Successfully subscribed to topic %s' %
self._control_manager_diagnostics_topic)
self._failed = False
def subscribe(self, topic):
if topic is None:
rospy.logerr("Got topic None, not subscribing")
return
if self.subscriber is not None:
self.subscriber.unregister()
self.last_img = None
type_name, topic_name, _ = get_topic_type(topic)
if type_name == 'sensor_msgs/Image':
self.subscriber = rospy.Subscriber(topic, Image, self.image_cb, queue_size=1)
self.bridge = CvBridge()
elif type_name == 'sensor_msgs/CompressedImage':
self.subscriber = rospy.Subscriber(topic, CompressedImage, self.image_cb, queue_size=1)
else:
rospy.logerr('Topic is not of image type')
return
while not rospy.is_shutdown() and self.last_img is None:
rospy.sleep(0.1)
if type_name == 'sensor_msgs/Image':
w = self.last_img.width
h = self.last_img.height
elif type_name == 'sensor_msgs/CompressedImage':
w, h = self.get_compressed_image_size(self.last_img)
self.img_w = w
self.img_h = h
self.last_topic = topic
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
Logger.loginfo('[IsRealsenseConnectedState] entering state')
self.last_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self.realsense_camera_info_topic)
if msg_topic == self.realsense_camera_info_topic:
msg_type = self._get_msg_from_path(msg_path)
self.realsense_subs_ = ProxySubscriberCached(
{self.realsense_camera_info_topic: msg_type})
self._connected = True
Logger.loginfo(
'[IsRealsenseConnectedState]: Successfully subscribed to previously unavailable topic %s'
% self.realsense_camera_info_topic)
else:
Logger.logwarn(
'[IsRealsenseConnectedState]: Topic %s still not available, giving up.'
% self.realsense_camera_info_topic)
if self._connected and self.realsense_subs_.has_msg(
self.realsense_camera_info_topic):
Logger.loginfo(
'[IsRealsenseConnectedState]: Waiting for msg from topic %s' %
self.realsense_camera_info_topic)
self.realsense_subs_.remove_last_msg(
self.realsense_camera_info_topic)
def _handle_add_topic_clicked(self):
selected_topic = str(self.topics_dropdown.currentText())
if selected_topic in self._exported_topics:
return
topic_type = rostopic.get_topic_type(selected_topic)[0]
publisher_info = {
'topic_name': str(selected_topic),
'type_name': str(topic_type),
'enabled': bool(True),
}
publisher_info['publisher_id'] = self._id_counter
self._id_counter += 1
publisher_info['counter'] = 0
publisher_info['enabled'] = publisher_info.get('enabled', False)
publisher_info['expressions'] = publisher_info.get('expressions', {})
publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name'])
if publisher_info['message_instance'] is None:
return
self.publisher_tree_widget.model().add_publisher(publisher_info)
self._exported_topics.append(selected_topic)
self._exported_publisher_info.append(publisher_info)
def subscribe(self, topic):
if topic is None:
rospy.logerr("Got topic None, not subscribing")
return
if self.subscriber is not None:
self.subscriber.unregister()
self.last_img = None
type_name, topic_name, _ = get_topic_type(topic)
if type_name == 'sensor_msgs/Image':
self.subscriber = rospy.Subscriber(topic,
Image,
self.image_cb,
queue_size=1)
self.bridge = CvBridge()
elif type_name == 'sensor_msgs/CompressedImage':
self.subscriber = rospy.Subscriber(topic,
CompressedImage,
self.image_cb,
queue_size=1)
else:
rospy.logerr('Topic is not of image type')
return
while not rospy.is_shutdown() and self.last_img is None:
rospy.sleep(0.1)
if type_name == 'sensor_msgs/Image':
w = self.last_img.width
h = self.last_img.height
elif type_name == 'sensor_msgs/CompressedImage':
w, h = self.get_compressed_image_size(self.last_img)
self.img_w = w
self.img_h = h
self.last_topic = topic
def addRobots(robots, regex, entries, pubsub):
'''
This adds the Entry in the complex robots dictionary
We iterate over each entry and initialize the robots-dict
appropiately. Then It is simply added.
robots: The dictionary robots[ROBOT_ID]["topics"][TOPIC_NAME] = {msg, type}
regex: The Regex we try to match in each entry
entries:The String Entries. Each element is in the following structure "/ROBOT_ID/TOPIC_NAME"
pubsub: A String. Either "publisher" or "subscriber"
'''
for entry in entries:
matches = re.search(regex, entry)
if matches is not None:
# We found a Match. Now add it to robots
robotID = matches.group(1)
topic = matches.group(2)
if robotID not in robots:
# Init robot Dict
robots[robotID] = {}
robots[robotID]["topics"] = {}
if topic not in robots[robotID]["topics"]:
# Init topic Dict
robots[robotID]["topics"][topic] = {}
# Get Message Type and convert it to Python-Specific Import
topic_type, _, _ = rostopic.get_topic_type(entry)
msg_type = topic_type.replace("/", ".msg.")
# Set it in robots
robots[robotID]["topics"][topic]["type"] = pubsub
robots[robotID]["topics"][topic]["msg"] = msg_type
def __init__(self, topic):
self.bridge = CvBridge()
self.last_frame = None
# Creating a window for later use
self.w_name = 'HSV color range chooser'
cv2.namedWindow('result')
# Creating track bar
cv2.createTrackbar('lh', 'result', 0, 179, nothing)
cv2.createTrackbar('ls', 'result', 0, 255, nothing)
cv2.createTrackbar('lv', 'result', 0, 255, nothing)
cv2.createTrackbar('hh', 'result', 0, 179, nothing)
cv2.createTrackbar('hs', 'result', 0, 255, nothing)
cv2.createTrackbar('hv', 'result', 0, 255, nothing)
cv2.setTrackbarPos('hh', 'result', 179)
cv2.setTrackbarPos('hs', 'result', 255)
cv2.setTrackbarPos('hv', 'result', 255)
type_name, topic_name, _ = get_topic_type(topic)
if type_name == 'sensor_msgs/Image':
self.subscriber = rospy.Subscriber(topic,
Image,
self.img_cb,
queue_size=1)
elif type_name == 'sensor_msgs/CompressedImage':
self.subscriber = rospy.Subscriber(topic,
CompressedImage,
self.img_cb,
queue_size=1)
rospy.loginfo("Subscribing to: " + self.subscriber.resolved_name)
def __init__(self, battery_status_topic, num_battery_cells):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(IsBatteryBellowValueState, self).__init__(input_keys=['cell_voltage_target'],
output_keys=['current_cell_voltage'],
outcomes=['is_bellow', 'is_ok']
)
self._topic = rospy.resolve_name(battery_status_topic)
self._num_battery_cells = num_battery_cells
# Store state parameter for later use.
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
#Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic))
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_battery = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('[IsBatteryBellowValueState]: Successfully subscribed to topic %s' % self._topic)
#brick_subs_ = rospy.Subscriber(self._topic, sensor_msgs.msg.BatteryState, self.battery_status_callback)
self.moving_average_hist_v_param = 0.1
self.volage_per_cell = 4.2
self.voltage = num_battery_cells * self.volage_per_cell
# The constructor is called when building the state machine, not when actually starting the behavior.
# Thus, we cannot save the starting time now and will do so later.
self._start_time = None
def __check_topic(self, topic_name):
topic_type, real_topic, msg_eval = rostopic.get_topic_type(topic_name) # (None, None, None)
if real_topic:
self._print_info("Checking topic '%s': OK" % topic_name)
else:
self.ALL_OK = False
self._print_fatal("Checking topic '%s': FAILED" % topic_name)
def action_check(self, params, userdata):
rospy.loginfo("Checking <<actions>>")
result_summary = {}
self.result = "success"
for item in params.values()[0]:
action_type = item['action_type']
action_name = item['action_name']
rospy.loginfo("Now checking <<%s>>", action_name)
rospy.loginfo("Of type <<%s>>", action_type)
#************
# Description: Get topic type and uses that for dynamically importing the required modules
imp_action = rostopic.get_topic_type("/"+action_name+"/goal", blocking=True)
imp_action = imp_action[0].split("/")[0]
imp_action += ".msg"
rospy.loginfo("Trying to import " + action_type + " from" +imp_action)
mod = __import__(imp_action, fromlist=[action_type]) # from move_base_msg.msg
cls = getattr(mod, action_type) # import MoveBaseAction
#****************************
ac_client = actionlib.SimpleActionClient(action_name, cls)
result = ac_client.wait_for_server(rospy.Duration(5))
result_summary[action_name] = result # this make a summary for the results for all the actions checked
if result == False:
self.result = "failed"
rospy.loginfo("Result of the Actions Check")
rospy.loginfo(result_summary)
rospy.loginfo("Finished Checking <<actions>>")
def goal_cb(self, msg):
goal=msg
print "Got Message"
if goal.marker_topic[0]=="/": #check if there is a '/' in front of the topic name so rostopic doesn't error
marker_type, real_name, fn=rostopic.get_topic_type(goal.marker_topic)
else:
marker_type, real_name, fn=rostopic.get_topic_type("/"+goal.marker_topic)
print (marker_type)
self.marker_repub=rospy.Publisher('ar_tag_pose', AlvarMarkers)
if marker_type == "ar_pose/ARMarker" and not self.defined:
print"Republisher started for AR Pose markers"
#Publish new goal message
self.send_new_goal(goal)
rospy.Subscriber(real_name, ARMarker, self.ar_pose_switch)
self.defined=True
def _get_topic_data_map(self, topic_data_map, topic_nm):
result = False
if (topic_nm in topic_data_map):
result = True
self.topic_items = topic_data_map[topic_nm]
else:
try:
topic_type_info = rostopic.get_topic_type(topic_nm)
if (topic_type_info[0] is not None):
topic_msg = rosmsg.get_msg_text(topic_type_info[0])
self.topic_items = self._parse_topic_items(
self.topic, topic_msg)
topic_data_map[topic_nm] = {
"topic_msg": topic_type_info[0],
"items": self.topic_items
}
result = True
except NameError as ne:
rospy.logerr("topic info get failed. %s", self.topic)
print(ne)
return result
def __init__(self, plan_keeper_mapped_arena_topic):
'''
Constructor
'''
super(WaitForArenaMappedObjectsState,
self).__init__(outcomes=['successed', 'failed'],
output_keys=['mapped_objects'])
self._topic = rospy.resolve_name(plan_keeper_mapped_arena_topic)
#self._sub = rospy.Subscriber(self._topic, brick_mapping.msg.MappedArenaObjectsStamped, self.mapped_area_callback)
#self._connected = True
#
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
#Logger.loginfo('[IsBatteryBellowValueState]: topic %s msg_topic %s'%(self._topic,msg_topic))
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo(
'[WaitForArenaMappedObjectsState]: subscribing topic %s ' %
(self._topic))
self.received = False
self.mapped_objects = None
def topic_scan(config):
g = rosgraph.impl.graph.Graph()
g.update()
topics = map(str.strip, g.nt_nodes)
subs = collections.defaultdict(list)
pubs = collections.defaultdict(list)
for edge in g.nt_all_edges:
src = edge.start.strip()
dest = edge.end.strip()
if src in topics:
subs[src] = dest
if dest in topics:
pubs[dest] = src
for topic in topics:
type_name = rostopic.get_topic_type(topic)[0]
if type_name == 'sensor_msgs/PointCloud2' or type_name == 'sensor_msgs/LaserScan':
config.observation_sources[topic] = {'type': type_name}
elif type_name == 'nav_msgs/OccupancyGrid':
base = topic.split('/')[-1]
if base == 'costmap':
if 'local' in topic:
config.maps[topic] = 'Local Costmap'
else:
config.maps[topic] = 'Global Costmap'
else:
config.maps[topic] = 'Map'
elif type_name == 'nav_msgs/Path':
config.paths[topic] = 'Path'
elif type_name == 'geometry_msgs/PoseStamped':
if topic in subs:
config.goal = topic
def __init__(self, ref_frame, camera_topic, camera_frame):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(DetectPartCameraState,
self).__init__(outcomes=['continue', 'failed'],
output_keys=['pose'])
self.ref_frame = ref_frame
self._topic = camera_topic
self._camera_frame = camera_frame
self._connected = False
self._failed = False
# tf to transfor the object pose
self._tf_buffer = tf2_ros.Buffer(
rospy.Duration(10.0)) #tf buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
# Subscribe to the topic for the logical camera
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn(
'Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...'
% (self._topic, self.name, str(msg_topic)))
def topic_scan(config):
g = rosgraph.impl.graph.Graph()
g.update()
topics = map(str.strip, g.nt_nodes)
subs = collections.defaultdict(list)
pubs = collections.defaultdict(list)
for edge in g.nt_all_edges:
src = edge.start.strip()
dest = edge.end.strip()
if src in topics:
subs[src] = dest
if dest in topics:
pubs[dest] = src
for topic in topics:
type_name = rostopic.get_topic_type(topic)[0]
if type_name=='sensor_msgs/PointCloud2' or type_name=='sensor_msgs/LaserScan':
config.observation_sources[topic] = {'type': type_name}
elif type_name=='nav_msgs/OccupancyGrid':
base = topic.split('/')[-1]
if base == 'costmap':
if 'local' in topic:
config.maps[topic] = 'Local Costmap'
else:
config.maps[topic] = 'Global Costmap'
else:
config.maps[topic] = 'Map'
elif type_name=='nav_msgs/Path':
config.paths[topic] = 'Path'
elif type_name=='geometry_msgs/PoseStamped':
if topic in subs:
config.goal = topic
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
#Logger.logerr('[ServiceFollowTrajectory]: enter')
self._start_time = rospy.get_time()
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached({self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo('[ServiceFollowTrajectory]: Successfully subscribed to previously unavailable topic %s' % self._control_manager_diagnostics_topic)
else:
Logger.logwarn('[ServiceFollowTrajectory]: Topic %s still not available, giving up.' % self._control_manager_diagnostics_topic)
if self._connected and self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic):
Logger.loginfo('[ServiceFollowTrajectory]: Waiting for msg from topic %s' % self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic)
self._failed = False
# Check if the ProxyServiceCaller has been registered
if not self._srv_follow.is_available(self._service_topic_follow):
Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_follow))
self._failed = True
return
if not self._srv_set_trajectory.is_available(self._service_topic_set_trajectory):
Logger.logerr('[ServiceFollowTrajectory]: Topic \'{}\' not yet registered!'.format(self._service_topic_set_trajectory))
self._failed = True
return
try:
service_request_set_trajectory = mrs_msgs.srv.TrajectoryReferenceSrvRequest()#TrackerTrajectorySrvRequest()
trajectory_msg = mrs_msgs.msg.TrajectoryReference()
trajectory_msg.header = std_msgs.msg.Header()
trajectory_msg.header.stamp = rospy.Time.now()
trajectory_msg.header.seq = 0
trajectory_msg.header.frame_id = userdata.frame_id
trajectory_msg.use_heading = True
trajectory_msg.fly_now = True
trajectory_msg.loop = False
trajectory_msg.points = userdata.scanning_trajectory
service_request_set_trajectory.trajectory = trajectory_msg
service_result_set_trajectory = self._srv_set_trajectory.call(self._service_topic_set_trajectory,service_request_set_trajectory)
Logger.logwarn('[ServiceFollowTrajectory]: Called \'{}\' in ServiceFollowTrajectory set trejectory'.format(self._service_topic_set_trajectory))
if not service_result_set_trajectory.success:
self._failed = True
Logger.logwarn('[ServiceFollowTrajectory]: Calling \'{}\' was not successful'.format(self._service_topic_set_trajectory))
else:
Logger.loginfo('[ServiceFollowTrajectory]: Calling \'{}\' was successful'.format(self._service_topic_set_trajectory))
except Exception as e:
Logger.logerr('Failed to call \'{}\' service request: {}'.format(self._service_topic_follow, str(e)))
self._failed = True
def update_topicinfo(self,topics):
new_topic_type = []
for topic in topics:
topic_type, _, _ = rostopic.get_topic_type(topic)
if topic_type:
new_topic_type += [topic_type]
self.topicinfo[topic] = topic_type
self.update_idl(new_topic_type)
def __init__(self, set_yaw_service_topic,
control_manager_diagnostics_topic, odometry_topic):
# Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments.
super(ServiceRotateRelativeToGoToState,
self).__init__(input_keys=[
'goal_tracker_point', 'frame_id', 'yaw_relative_to_goal'
],
output_keys=['desired_yaw'],
outcomes=['successed', 'failed'])
# Store state parameter for later use.
self._set_yaw_service_topic = rospy.resolve_name(set_yaw_service_topic)
self._control_manager_diagnostics_topic = rospy.resolve_name(
control_manager_diagnostics_topic)
self._odometry_topic = rospy.resolve_name(odometry_topic)
# Create proxy service client
self._srv = ProxyServiceCaller(
{self._set_yaw_service_topic: mrs_msgs.srv.Vec1})
(msg_path, msg_topic,
fn) = rostopic.get_topic_type(self._control_manager_diagnostics_topic)
if msg_topic == self._control_manager_diagnostics_topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub_cont_diag = ProxySubscriberCached(
{self._control_manager_diagnostics_topic: msg_type})
self._connected = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._control_manager_diagnostics_topic)
(msg_path_odom, msg_topic_odom,
fn_odom) = rostopic.get_topic_type(self._odometry_topic)
if msg_topic_odom == self._odometry_topic:
msg_type_odom = self._get_msg_from_path(msg_path_odom)
self._sub_odom = ProxySubscriberCached(
{self._odometry_topic: msg_type_odom})
self._connected_odom = True
Logger.loginfo(
'[ServiceRotateRelativeToGoToState]: Successfully subscribed to topic %s'
% self._odometry_topic)
self._desired_yaw = 0
self._sended = False
self._failed = False
def test_get_topic_type(self):
import rostopic
self.assertEquals((None, None, None),
rostopic.get_topic_type('/fake', blocking=False))
t, n, f = rostopic.get_topic_type('/rosout', blocking=False)
self.assertEquals('rosgraph_msgs/Log', t)
self.assertEquals('/rosout', n)
self.assert_(f is None)
t, n, f = rostopic.get_topic_type('/rosout/name', blocking=False)
self.assertEquals('rosgraph_msgs/Log', t)
self.assertEquals('/rosout', n)
self.failIf(f is None)
from rosgraph_msgs.msg import Log
self.assertEquals("bob", f(Log(name="bob")))
def check_topic(userdata, topic_name):
""" Check if is some node publishing on a topic """
topic_name = str(topic_name)
topic_type, real_topic, msg_eval = get_topic_type(topic_name) # (None, None, None)
if real_topic:
rospy.loginfo("Checking topic '%s': OK" % topic_name)
return succeeded
rospy.loginfo("Checking topic '%s': FAILED" % topic_name)
return aborted
def get_topic_type(self, topic_name, clb=None):
"""Returns a list of currently alive ROS Topics. Similar `rostopic type <topic_type>`.
Args:
topic_name (str): The Topic name.
clb (function, optional): Callback function that is called upon response is received.
"""
topic_type = rostopic.get_topic_type(topic_name)
return topic_type
def generate_type_info(self):
'''
Basic connection details are provided by get system state from the master, which is
a one shot call to give you information about every connection possible. it does
not however provide type info information and the only way of retrieving that from
the master is making one xmlrpc call to the master for every single connection.
This gets expensive, so generating this information is usually delayed until we
need it and done via this method.
'''
if self.type_info is None:
if self.type == PUBLISHER or self.type == SUBSCRIBER:
self.type_info = rostopic.get_topic_type(self.name)[0] # message type
elif self.type == SERVICE:
self.type_info = rosservice.get_service_uri(self.name)
elif self.type == ACTION_SERVER or self.type == ACTION_CLIENT:
goal_topic = self.name + '/goal'
goal_topic_type = rostopic.get_topic_type(goal_topic)
self.type_info = re.sub('ActionGoal$', '', goal_topic_type[0]) # Base type for action
def _type(topic):
topic = '/' + topic
try:
_topic_type, _, _ = rt.get_topic_type(topic)
if _topic_type:
return response_ok({'topic': topic, 'message_type': _topic_type})
else:
return response_error("Topic '{:s}' not found".format(topic))
except Exception as e:
return response_error(str(e))
def listener(self,fd,topicName):
fo = os.fdopen(int(fd), "w", 0)
self.data.append(fo)
rosmsg = get_topic_type(topicName)[0]
rosmsg = rosmsg.split("/")
module = import_module(rosmsg[0]+".msg")
rospy.init_node('listener', anonymous=True)
rospy.Subscriber(topicName, getattr(module,rosmsg[1]), self.callback)
rospy.spin()
def test_lg_media_topic_presence(self):
"""
Check lg_media topic is there.
"""
#master = rosgraph.masterapi.Master("caller")
#topics = master.getTopicTypes()
#assert ["/media_service/left_one", "lg_media/AdhocMedias"] in topics
topic_type, real_topic, msg_eval = rostopic.get_topic_type(TOPIC_NAME, blocking=False)
assert topic_type is not None, "Topic not found: {}".format(TOPIC_NAME)
assert topic_type == "%s/AdhocMedias" % ROS_NODE_NAME
def main():
#rospy.init_node('goal_sender')
argv = sys.argv
#argv = rospy.myargv(argv)
from optparse import OptionParser
parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
parser.add_option("-f",
dest="filename", default=None,
help="Name of yaml file name of options")
parser.add_option("-w",
dest="timeout", default=3, type="float",
help="time to wait for server and response")
parser.add_option("-t",
dest="topicname", default=None)
parser.add_option("-r",
dest="responsetopic", default=None)
parser.add_option("--topic-type",
dest="topictype", default=None)
(options, args) = parser.parse_args(argv)
if not options.filename or not options.topicname:
rospy.logfatal("No yaml file name given")
sys.exit(-1)
topictype, topicname, fn = rostopic.get_topic_type(options.topicname, False)
if not topictype:
rospy.logerror("Topic not currently subscribed %s"%topicname)
if not options.topictype and not topictype:
exit(-1)
if options.topictype and topictype and options.topictype is not topictype:
rospy.logerror("Topic type does not agree: topic %s, given type: %s, detected type: %s"
% (topicname, options.topictype, topictype))
exit(-1)
if options.topictype:
topictype = options.topictype
pub, msg_class = rostopic.create_publisher(options.topicname, topictype, False)
rostopic.stdin_publish(pub, msg_class, 1, True, options.filename, True)
if options.responsetopic:
subtype, responsetopic, fn = rostopic.get_topic_class(options.responsetopic, False)
msg = rospy.wait_for_message(options.responsetopic, subtype, options.timeout)
print(msg)
def _init_type(self):
"""
Lazy-initialize type based on graph state.
"""
if self._ns == '/':
return
if self._type is None:
self._type_name = rostopic.get_topic_type(self._ns, blocking=False)[0]
if self._type_name:
self._type = roslib.message.get_message_class(self._type_name)
if self._type is None:
pkg, base_type = roslib.names.package_resource_name(self._type_name)
print >> sys.stderr, "\ncannot retrieve type [%s].\nPlease type 'rosmake %s'"%(self._type_name, pkg)
def on_enter(self, userdata):
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic)
else:
Logger.logwarn('Topic %s still not available, giving up.' % self._topic)
if self._connected and self._clear and self._sub.has_msg(self._topic):
self._sub.remove_last_msg(self._topic)
def get_goal_type(action):
"""Gets the corresponding ROS action goal type.
Args:
action: ROS action name.
Returns:
Goal message type. None if not found.
"""
msg_type = rostopic.get_topic_type("{}/goal".format(action))[0]
# Replace 'ActionGoal' with 'Goal'.
return msg_type[:-10] + "Goal"
def get_result_type(action):
"""Gets the corresponding ROS action result type.
Args:
action: ROS action name.
Returns:
Result message type. None if not found.
"""
msg_type = rostopic.get_topic_type("{}/result".format(action))[0]
# Replace 'ActionResult' with 'Result'.
return msg_type[:-12] + "Result"
def get_feedback_type(action):
"""Gets the corresponding ROS action feedback type.
Args:
action: ROS action name.
Returns:
Feedback message type. None not found.
"""
msg_type = rostopic.get_topic_type("{}/feedback".format(action))[0]
# Replace 'ActionFeedback' with 'Feedback'.
return msg_type[:-14] + "Feedback"
def process_topics(self, tfs):
the_type = rostopic.get_topic_type(tfs, blocking=True)[0]
the_type = the_type.split("/")
the_type[0] += ".msg"
mod = __import__(the_type[0], fromlist=[the_type[1]])
cls = getattr( mod , the_type[1] )
msg = rospy.wait_for_message(tfs, cls)
return msg
def add_topic(self, topic_name, ws_name=None, topic_type=None, allow_pub=True, allow_sub=True):
resolved_topic_name = rospy.resolve_name(topic_name)
if topic_type is None:
topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name)
if not topic_type:
print 'Unknown topic %s' % topic_name
return False
if ws_name is None:
ws_name = topic_name
if ws_name.startswith('/'):
ws_name = ws_name[1:]
self.topics[ws_name] = Topic(topic_name, topic_type, allow_pub=allow_pub, allow_sub=allow_sub)
return True
def add_action(self, action_name, ws_name=None, action_type=None):
if action_type is None:
resolved_topic_name = rospy.resolve_name(action_name + '/result')
topic_type, _, _ = rostopic.get_topic_type(resolved_topic_name)
if not topic_type:
print 'Unknown action %s' % action_name
return False
action_type = topic_type[:-len('ActionResult')]
if ws_name is None:
ws_name = action_name
if ws_name.startswith('/'):
ws_name = ws_name[1:]
self.actions[ws_name] = Action(action_name, action_type)
return True
def __init__(self, topic, msg_type=None, latched_client_id=None, queue_size=100):
""" Register a publisher on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the publisher to
msg_type -- (optional) the type to register the publisher as. If not
provided, an attempt will be made to infer the topic type
latch -- (optional) if a client requested this publisher to be latched,
provide the client_id of that client here
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the publisher and associated member variables
self.clients = {}
self.latched_client_id = latched_client_id
self.topic = topic
self.msg_class = msg_class
self.publisher = Publisher(topic, msg_class, latch=(latched_client_id!=None), queue_size=queue_size)
self.listener = PublisherConsistencyListener()
self.listener.attach(self.publisher)
def _init_action(self):
if self._ns == '/':
return
if self._type is None:
#TODO: shift away from feedback, use status instead
feedback_topic = roslib.names.ns_join(self._ns, 'feedback')
feedback_type_name = rostopic.get_topic_type(feedback_topic, blocking=False)[0]
if feedback_type_name and feedback_type_name.endswith('Feedback'):
self._type_name = feedback_type_name[:-len('Feedback')]
if not self._type_name.endswith('Action'):
# abort init
self._type_name = None
else:
# rosh's loader has extra toys
self._type = rosh.impl.msg.get_message_class(self._type_name)
if self._type is not None:
# only load if the previous load succeeded
self._goal = rosh.impl.msg.get_message_class(self._type_name[:-len('Action')]+'Goal')
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(service_name)
tool_tip += '\n %s [%s]' % (service_name, service_type)
except rosservice.ROSServiceIOException, e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
def get_action_class(action):
"""Gets the corresponding ROS action class.
Args:
action: ROS action name.
Returns:
Action message class. None if not found.
"""
goal_msg_type = rostopic.get_topic_type("{}/goal".format(action))[0]
# Verify goal message was found.
if not goal_msg_type:
return None
# Goal message name is the same as the action message name + 'Goal'.
action_msg_type = goal_msg_type[:-4]
return roslib.message.get_message_class(action_msg_type)
def add_trigger(topic, expr, select, index):
topic_type, _, _ = rostopic.get_topic_type(topic)
topic_class, _, _ = rostopic.get_topic_class(topic)
if(topic_type == None):
rospy.loginfo('%s is not published yet', topic)
return None
if(topic_class == None):
rospy.loginfo('%s is not builded yet', topic_type)
return None
try:
cb_obj = gen_callback(expr, select, index)
rospy.loginfo("start subscribe (topic=%s type=%s)", topic, topic_type);
sub = rospy.Subscriber(topic,topic_class,cb_obj)
except Exception, e:
rospy.loginfo(str(e))
return None
def __init__(self, topic, blocking = True, clear = False):
'''
Constructor
'''
super(SubscriberState, self).__init__(outcomes=['received', 'unavailable'],
output_keys=['message'])
self._topic = topic
self._blocking = blocking
self._clear = clear
self._connected = False
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic)
if msg_topic == self._topic:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic: msg_type})
self._connected = True
else:
Logger.logwarn('Topic %s for state %s not yet available.\nFound: %s\nWill try again when entering the state...' % (self._topic, self.name, str(msg_topic)))
def __init__(self, topic_name, bagfile, continuous=False):
# this gets the topics type using the roslib
the_type = rostopic.get_topic_type(topic_name, blocking=True)[0]
the_type = the_type.split("/")
the_type[0] += ".msg"
# this imports the necessary modules and instantiates the necessary object
mod = __import__(the_type[0], fromlist=[the_type[1]])
cls = getattr( mod , the_type[1] )
self.msg = None
self.topic_name = topic_name
self.topic_type = cls
self.continuous = continuous
self.bagfile = bagfile
# this creates the subscriber for the specific topic
rospy.Subscriber(self.topic_name, self.topic_type, self.callback)
global_lock.locked = False
def __init__(self, topic, msg_type=None):
""" Register a subscriber on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the subscriber on
msg_type -- (optional) the type to register the subscriber as. If not
provided, an attempt will be made to infer the topic type
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the subscriber and associated member variables
self.subscriptions = {}
self.lock = Lock()
self.topic = topic
self.msg_class = msg_class
self.subscriber = Subscriber(topic, msg_class, self.callback)
def get_field_type(topic_name):
"""
Get the Python type of a specific field in the given registered topic.
If the field is an array, the type of the array's values are returned and the is_array flag is set to True.
This is a static type check, so it works for unpublished topics and with empty arrays.
:param topic_name: name of field of a registered topic, ``str``, i.e. '/rosout/file'
:returns: field_type, is_array
"""
# get topic_type and message_evaluator
topic_type, real_topic_name, _ = get_topic_type(topic_name)
if topic_type is None:
#qDebug('topic_helpers.get_field_type(%s): get_topic_type failed' % (topic_name))
return None, False
message_class = roslib.message.get_message_class(topic_type)
if message_class is None:
qDebug('topic_helpers.get_field_type(%s): get_message_class(%s) failed' % (topic_name, topic_type))
return None, False
slot_path = topic_name[len(real_topic_name):]
return get_slot_type(message_class, slot_path)
def process_topics(self, tfs):
# process the topics
# this gets the topics type using the roslib
the_type = rostopic.get_topic_type(tfs, blocking=True)[0]
the_type = the_type.split("/")
the_type[0] += ".msg"
# this imports the necessary modules and instantiates the necessary object
mod = __import__(the_type[0], fromlist=[the_type[1]])
cls = getattr(mod, the_type[1])
timeout = 3.0
try:
if tfs == "/joint_states":
msg = self.ja.jointsMsg
# rospy.loginfo("joint Message")
# rospy.loginfo(msg)
elif tfs == "/tf":
# self.tfa.process_tfs()
transf = self.tfa.transforms
self.tf_write(transf)
msg = Empty()
else:
msg = rospy.wait_for_message(tfs, cls, timeout)
except rospy.ROSException:
# an empty message is created due to an exception for preventing breaks
# on the recording process
rospy.logwarn("skipping topic: " + str(tfs))
msg = Empty()
return msg
