def __getpubsubsrv(_nodename):
"""
Get Pub, Subs and Srvs of ROS Node
:param _nodename:
:return:
"""
try:
_desc = rosnode.get_node_info_description(_nodename)
except:
_desc = ''
_publines = []
_sublines = []
_serlines = []
_swtch = 0
for _line in _desc.splitlines():
if len(_line) > 0:
if 'Publications' in _line:
_swtch = 1
elif 'Subscriptions' in _line:
_swtch = 2
elif 'Services' in _line:
_swtch = 3
else:
if _swtch == 1:
_publines.append(str(_line))
elif _swtch == 2:
_sublines.append(str(_line))
elif _swtch == 3:
_serlines.append(str(_line))
return _publines, _sublines, _serlines
def initialise_node(self):
nodeinfo = rosnode.get_node_info_description(self.name)
lines = nodeinfo.split("\n")
count = 0
line = lines[count]
while not line.startswith("Publications:") and count < len(lines):
count += 1
line = lines[count]
print line
count += 1
line = lines[count]
while line.strip().startswith("*") and count < len(lines):
# new published topic
topic = self.get_topic(line.rstrip())
msg = self.get_msg(line.rstrip())
pub_topic = {"topic":topic,"msg":msg}
self.published_topics.append(pub_topic)
print "This is a published topic: %s with msg:%s" % (topic,msg)
count += 1
line = lines[count]
while not line.startswith("Subscriptions:") and count < len(lines):
count += 1
line = lines[count]
count += 1
line = lines[count]
while line.strip().startswith("*") and count < len(lines):
# new subscribed topic
topic = self.get_topic(line.rstrip())
msg = self.get_msg(line.rstrip())
sub_topic = {"topic":topic,"msg":msg}
self.subscribed_topics.append(sub_topic)
print "This is a subscribed topic: %s with msg:%s" % (topic,msg)
count += 1
line = lines[count]
while not line.startswith("Services:") and count < len(lines):
count += 1
line = lines[count]
count += 1
line = lines[count]
while line.strip().startswith("*") and count < len(lines):
# new subscribed topic
srv = self.get_service(line)
srv_type = self.get_service_type(srv)
service = {"name":srv,"msg":srv_type}
self.services.append(service)
print "This is service: %s with type:%s" % (srv, srv_type)
count += 1
line = lines[count]
def get_info_text(selection_url):
if selection_url is None:
return ''
if selection_url.startswith('node:'):
try:
node_name = selection_url[5:]
master = roslib.scriptutil.get_master()
node_api = rosnode.get_api_uri(master, node_name)
return rosnode.get_node_info_description(node_name) + rosnode.get_node_connection_info_description(node_api)
except rosnode.ROSNodeException, e:
return "ERROR: %s"%str(e)
def get_info_text(selection_url):
if selection_url is None:
return ''
if selection_url.startswith('node:'):
try:
node_name = selection_url[5:]
master = rosgraph.Master('/rxgraph')
node_api = rosnode.get_api_uri(master, node_name)
return rosnode.get_node_info_description(
node_name) + rosnode.get_node_connection_info_description(
node_api, master)
except rosnode.ROSNodeException, e:
return "ERROR: %s" % str(e)
def __init__(self):
types = {
'CameraInfo': CameraInfo,
'Image': Image,
'TFMessage': TFMessage,
'InteractiveMarkerUpdate': InteractiveMarkerUpdate
}
rospy.init_node(f"rosbag_recorder", anonymous=False)
config_path = rospy.get_param("~config")
print(config_path)
sdk = bosdyn.client.create_standard_sdk('understanding-spot')
self.robot = sdk.create_robot('192.168.50.3')
with open("/home/spot/config/config") as f:
for line in f:
line = line.split('/')
user = line[0].strip()
password = line[1].strip()
pinata_pub = line[2].strip()
pinata_secret = line[3].strip()
self.pinata = PinataPy(pinata_pub, pinata_secret)
self.robot.authenticate(user, password)
self.state_client = self.robot.ensure_client('robot-state')
inf = rosnode.get_node_info_description('/rviz')
inf = inf.split()
# self.topics_str = ''
# types_name = []
# topic = False
# for p in inf:
# p = p.strip()
# if topic:
# if p[0] == '/':
# self.command_full.append(p)
# self.topics_str += f'{p} '
# # if p[0] == '[':
# # types_name.append(p.split('/')[-1][:-1])
# if p == "Subscriptions:":
# topic = True
# if p == "Services:":
# topic = False
# print(self.command_full)
# print(types)
self.recording = False
i = 0
self.bag = None
def refresh(self, state):
self.info_text = rosnode.get_node_info_description(self.name)
self.setToolTip(self.info_text)
self.pubs = [self.ui.topics[t] for t, l in state[0] if ((self.name in l) and (self.ui.topics.has_key(t)))]
self.subs = [self.ui.topics[t] for t, l in state[1] if ((self.name in l) and (self.ui.topics.has_key(t)))]
def initialise_node(self):
nodeinfo = rosnode.get_node_info_description(self.name)
lines = nodeinfo.split("\n")
count = 0
line = lines[count]
while not line.startswith("Publications:") and count < len(lines):
count += 1
line = lines[count]
count += 1
line = lines[count]
while line.strip().startswith("*") and count < len(lines):
# new published topic
topic = self.get_topic(line.rstrip())
msg = self.get_topic_type(topic)
# msg = self.get_msg(line.rstrip())
pub_topic = {'topic': topic, 'msg': msg}
self.published_topics.append(pub_topic)
# print "This is a published topic: %s with msg:%s" % (topic,msg)
count += 1
line = lines[count]
while not line.startswith("Subscriptions:") and count < len(lines):
count += 1
line = lines[count]
count += 1
line = lines[count]
while line.strip().startswith("*") and count < len(lines):
# new subscribed topic
topic = self.get_topic(line.rstrip())
# info_text = rostopic.get_info_text(topic)
msg = self.get_topic_type(topic)
# msg = self.get_msg(line.rstrip())
if topic == "/move_base_simple/goal":
print "RosNode has found %s" % topic
sub_topic = {"topic": topic, "msg": msg}
self.subscribed_topics.append(sub_topic)
# print "This is a subscribed topic: %s with msg:%s" % (topic,msg)
count += 1
line = lines[count]
while not line.startswith("Services:") and count < len(lines):
count += 1
line = lines[count]
count += 1
line = lines[count]
while line.strip().startswith("*") and count < len(lines):
# new subscribed topic
srv = self.get_service(line)
srv_type = self.get_service_type(srv)
service = {"name": srv, "msg": srv_type}
self.services.append(service)
# print "This is service: %s with type:%s" % (srv, srv_type)
count += 1
line = lines[count]
def get_node_details(self, node_name, clb=None):
"""Return detailes of given node. Include list of subscribers, publishers and services."""
node_details = rosnode.get_node_info_description(node_name)
return node_details
def validate_control_plugins(self):
"""
Conduct validation checks for each control plugin's node (as specified by this node's
configuration parameters) for proper publications, subscriptions, and advertised services. Based on the
results, this function updates each control plugin's ControlPluginResults object accordingly.
"""
for plugin_name, plugin_results_object in self.control_plugin_validation_results.items(
):
plugin_node_name = plugin_results_object.node_name
rospy.loginfo(
"Processing publishers, subscribers, and services for " +
str(plugin_name) + " (Control Plugin)")
# Check whether the node has been created
if rosnode.rosnode_ping(plugin_node_name, max_count=5):
plugin_results_object.requirement_results.has_node = True
rospy.loginfo("Success: Node " + str(plugin_node_name) +
" exists.")
else:
rospy.logerr("ERROR: No node response for " +
str(plugin_node_name) + ". Node does not exist.")
# Obtain string that includes information regarding a node's publications, subscriptions, and services
rosnode_info_string = (
rosnode.get_node_info_description(plugin_node_name))
# Get substring from rosnode info that contains 'Subscriptions' information
sub_index_start = rosnode_info_string.index("Subscriptions:")
sub_index_end = rosnode_info_string.index("Services:")
subscriptions_string = rosnode_info_string[
sub_index_start:sub_index_end]
# Check for required and optional subscriptions
if plugin_results_object.requirement_results.plan_trajectory_topic in subscriptions_string:
plugin_results_object.requirement_results.has_plan_trajectory_sub = True
rospy.loginfo("Success: " + str(plugin_node_name) +
" subscribes to " +
str(plugin_results_object.requirement_results.
plan_trajectory_topic))
else:
rospy.logerr("ERROR: " + str(plugin_node_name) +
" does not subscribe to " +
str(plugin_results_object.requirement_results.
plan_trajectory_topic))
# Get substring from rosnode info that contains 'Publications' information
pub_index_start = rosnode_info_string.index("Publications:")
pub_index_end = rosnode_info_string.index("Subscriptions:")
publications_string = rosnode_info_string[
pub_index_start:pub_index_end]
# Check for required and optional publications
if plugin_results_object.requirement_results.plugin_discovery_topic in publications_string:
plugin_results_object.requirement_results.has_plugin_discovery_pub = True
rospy.loginfo("Success: " + str(plugin_node_name) +
" publishes to " +
str(plugin_results_object.requirement_results.
plugin_discovery_topic))
else:
rospy.logerr("ERROR: " + str(plugin_node_name) +
" does not publish to " +
str(plugin_results_object.requirement_results.
plugin_discovery_topic))
if plugin_results_object.requirement_results.final_waypoints_topic in publications_string:
plugin_results_object.requirement_results.has_final_waypoints_pub = True
rospy.loginfo("Success: " + str(plugin_node_name) +
" publishes to " +
str(plugin_results_object.requirement_results.
final_waypoints_topic))
else:
rospy.logerr("ERROR: " + str(plugin_node_name) +
" does not publish to " +
str(plugin_results_object.requirement_results.
final_waypoints_topic))
return