def _wait_for_master(self):
while True:
time.sleep(0.1)
if not self._wait_for_master_dialog:
break
try:
rospy.get_master().getSystemState()
except Exception:
continue
self._master_found_signal.emit(QMessageBox.Ok)
break
def update_topic_list(self):
self.topic_combo_box.clear()
self.topic_combo_box.setEnabled(False)
self.topic_combo_box.blockSignals(True)
self.topic_combo_box.addItem('Updating...')
# get topic list
_, _, topic_type = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_type)
# filter list
topic_dict_filtered = dict()
for k, v in topic_dict.items():
if (len(topic_type) == 0) or (v == self.topic_type):
if (self.is_action_topic):
topic_dict_filtered[k[:-5]] = v
else:
topic_dict_filtered[k] = v
self.topic_combo_box.clear()
self.topic_combo_box.addItems(sorted(topic_dict_filtered.keys()))
if (self.topic_combo_box.count() > 0):
self.topic_combo_box.setEnabled(True)
self.topic_combo_box.blockSignals(False)
self.topic_changed(self.topic_combo_box.currentText())
else:
self.topic_combo_box.addItem('No topics available!')
def isMasterAlive():
"""
return True if master alive and return False if
master is not alive
"""
global previous_run_id
try:
# first check the host is available
master_host = urlparse.urlsplit(rospy.core.rosgraph.get_master_uri()).hostname
response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null")
if response != 0:
print "master machine looks down"
return False
assert 1 == rospy.get_master().getSystemState()[0]
run_id = rospy.get_param("/run_id")
if not previous_run_id:
previous_run_id = run_id
if run_id != previous_run_id:
print "run_id is not same"
previous_run_id = run_id
return False
return True
except Exception, e:
return False
def setUp(self):
# first we setup our publishers and our node (used by rospy.resolve_name calls to remap topics)
self.strpub = rospy.Publisher('/test/string', String, queue_size=1)
self.emppub = rospy.Publisher('/test/empty', Empty, queue_size=1)
self._master = rospy.get_master()
self.service_if_pool = RosServiceIfPool()
# we need to speed fast enough for the tests to not fail on timeout...
rospy.set_param('/connection_cache/spin_freq', 2) # 2 Hz
self.connection_cache_node = roslaunch.core.Node('rocon_python_comms', 'connection_cache.py',
name='connection_cache',
remap_args=[('~list', rospy.resolve_name('~connections_list')),
(
'~diff', rospy.resolve_name('~connections_diff'))])
# Easier to remap the node topic to the proxy ones, instead of the opposite, since there is no dynamic remapping.
# However for normal usecase, remapping the proxy handles is preferable.
try:
self.connection_cache_proc = self.launch.launch(self.connection_cache_node)
except roslaunch.RLException as rlexc:
raise nose.SkipTest("Connection Cache Node not found (part of rocon_python_comms pkg). Skipping test.")
assert self.connection_cache_proc.is_alive()
# wait for node to be started
node_api = None
with Timeout(5) as t:
while not t.timed_out and node_api is None:
node_api = rosnode.get_api_uri(rospy.get_master(), 'connection_cache')
assert node_api is not None # make sure the connection cache node is started before moving on.
self.connection_cache = connection_cache_proxy_create()
def _get_cart_status(self):
"""
If cart is not supposed to be attached, returns True
If cart is supposed to be attached, checks last known distance of cart and
returns True if cart is within threshold
:returns: (bool, bool)
"""
is_cart_attached = False
try:
rospy.get_master().getPid()
is_cart_attached = rospy.get_param(self.cart_attached_param_name, False)
except:
print('[cart_attachment_visual_3d_monitor] ROS master not running')
if not is_cart_attached:
return True, True
if self.cart_state['cart_pose'] is None or \
(time.time() - self.cart_state['last_received_time']) > 10.0:
self.toggle_cart_publisher_client = rospy.ServiceProxy(self.toggle_publisher_srv_name, ToggleObjectPublisher)
self.toggle_cart_publisher_client(enable_publisher=True)
return False, False
dist = np.linalg.norm([self.cart_state['cart_pose'].position.x, self.cart_state['cart_pose'].position.y])
if (dist < self.nominal_plane_distance + self.plane_distance_tolerance and
dist > self.nominal_plane_distance - self.plane_distance_tolerance):
return True, True
else:
return False, False
def main():
parser = argparse.ArgumentParser(
description='A script to test moveit planners. '
'Should be run after launching the UR10 arm and shadow hand: \n '
'roslaunch sr_robot_launch sr_right_ur10arm_hand.launch OR \n '
'roslaunch sr_robot_launch sr_left_ur10arm_hand.launch',
add_help=True,
usage='%(prog)s [group_id]',
formatter_class=argparse.RawTextHelpFormatter)
parser.add_argument(dest='group_id',
help="Should be either right_arm or left_arm")
parser.parse_args()
group_id = str(sys.argv[1])
try:
rospy.get_master().getPid()
except Exception:
print("Please launch robot.")
sys.exit()
# sleep to wait for robot description to be loaded
while not rospy.search_param(
'robot_description_semantic') and not rospy.is_shutdown():
print("Waiting for robot description, has robot been launched?")
time.sleep(0.5)
planner_list = [
"BKPIECEkConfigDefault", "ESTkConfigDefault", "KPIECEkConfigDefault",
"LBKPIECEkConfigDefault", "PRMkConfigDefault", "RRTkConfigDefault",
"SBLkConfigDefault", "PRMstarkConfigDefault", "RRTstarkConfigDefault"
]
for planner in planner_list:
TestPlanners(group_id, planner)
def _check_for_master(self):
# check if master is available
try:
rospy.get_master().getSystemState()
return
except Exception:
pass
# spawn thread to detect when master becomes available
self._wait_for_master_thread = threading.Thread(
target=self._wait_for_master)
self._wait_for_master_thread.start()
self._wait_for_master_dialog = QMessageBox(
QMessageBox.Question, self.tr('Waiting for ROS master'),
self.
tr("Could not find ROS master. Either start a 'roscore' or abort loading the plugin."
), QMessageBox.Abort)
self._master_found_signal.connect(self._wait_for_master_dialog.done,
Qt.QueuedConnection)
button = self._wait_for_master_dialog.exec_()
# check if master existence was not detected by background thread
no_master = button != QMessageBox.Ok
self._wait_for_master_dialog.deleteLater()
self._wait_for_master_dialog = None
if no_master:
raise PluginLoadError(
'RosPyPluginProvider._init_node() could not find ROS master')
def __is_master_running(self):
'''Returns True if a ROS master is running; returns False otherwise.
'''
try:
rospy.get_master().getPid()
return True
except:
return False
def check_Ros_Master_status():
import rospy
status = True
try:
rospy.get_master().getPid()
except:
status = False
return status
def setUpClass(cls):
cls.core = subprocess.Popen(['roscore'])
rospy.init_node('test_driver')
for _ in range(5):
try:
rospy.get_master()
break
except Exception as _: #pylint: disable=broad-except
time.sleep(0.5)
def _wait_for_master(self):
while True:
time.sleep(0.1)
if not self._wait_for_master_dialog:
break
try:
rospy.get_master().getSystemState()
except Exception:
continue
self._master_found_signal.emit(QMessageBox.Ok)
break
def SafeConnect():
try:
print "Cerium is connecting to ROS, please wait."
rospy.get_master().getPid()
# If an exception was not thrown, establish a connection.
status = rospy.init_node('listener', anonymous=True)
print "Cerium is connected to ROS!"
return status
except:
print "Cerium cannot start due to a roscore issue."
print "Please check the roscore configuration."
exit()
def get_node_info(self, event):
"""
Get the list of all Nodes running on the host.
Update the __node_list
"""
if not self.__is_enabled:
pass
if self.__is_enabled:
nodes = []
for node_name in rosnode.get_node_names():
try:
node_api = rosnode.get_api_uri(rospy.get_master(),
node_name,
skip_cache=True)
if self._id in node_api[2]:
nodes.append(node_name)
except:
pass
for node in nodes:
if node not in self.__node_list:
"""TODO currently not catching the exception here - master not running is a hard error so it does
not make sense to continue running.."""
node_api = rosnode.get_api_uri(rospy.get_master(),
node,
skip_cache=True)
try:
pid = self.get_node_pid(node_api, node)
if not pid:
continue
node_process = psutil.Process(pid)
new_node = NodeStatisticsHandler(
self._id, node, node_process)
self.__dict_lock.acquire(True)
self.__node_list[node] = new_node
self.__dict_lock.release()
except psutil.NoSuchProcess:
rospy.loginfo('pid of node %s could not be fetched' %
node)
continue
self.__dict_lock.acquire()
to_remove = []
for node_name in self.__node_list:
if node_name not in nodes:
rospy.logdebug('removing %s from host' % node_name)
to_remove.append(node_name)
for node_name in to_remove:
self.remove_node(node_name)
self.__dict_lock.release()
def WaitForRoscoreStart(self):
bRoscoreFail = True
print(bcolors.OKBLUE + "===========Waiting For Roscore=========" +
bcolors.ENDC)
while bRoscoreFail == True:
bRoscoreFail = False
try:
rospy.get_master().getPid()
except:
bRoscoreFail = True
rospy.sleep(1)
print(bcolors.OKBLUE + "===========Roscore Running=========" +
bcolors.ENDC)
def WaitForRoscoreClose(self):
bRoscoreRunning = True
print(bcolors.OKBLUE +
"===========Waiting For Roscore to Close=========" +
bcolors.ENDC)
while bRoscoreRunning == True:
try:
rospy.get_master().getPid()
except:
bRoscoreRunning = False
rospy.sleep(1)
print(bcolors.OKBLUE + "===========Roscore Closed=========" +
bcolors.ENDC)
def get_status(self):
status_msg = self.get_status_message_template()
status_msg['monitorName'] = self.config_params.name
status_msg['monitorDescription'] = self.config_params.description
status_msg['healthStatus'] = dict()
master_running = True
try:
rospy.get_master().getPid()
except:
master_running = False
status_msg['healthStatus']['status'] = master_running
return status_msg
def isMasterAlive():
"""
return True if master alive and return False if
master is not alive
"""
global previous_run_id
try:
# first check the host is available
master_host = urlparse.urlsplit(
rospy.core.rosgraph.get_master_uri()).hostname
response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null")
if response != 0:
print "master machine looks down"
return False
assert 1 == rospy.get_master().getSystemState()[0]
run_id = rospy.get_param("/run_id")
if not previous_run_id:
previous_run_id = run_id
if run_id != previous_run_id:
print "run_id is not same"
previous_run_id = run_id
return False
return True
except Exception, e:
return False
def setUp(self):
# we need to speed fast enough for the tests to not fail on timeout...
rospy.set_param('/connection_cache/spin_freq', 2) # 2 Hz
self.connection_cache_node = roslaunch.core.Node(
'rocon_python_comms',
'connection_cache.py',
name='connection_cache',
remap_args=[
('~list', '/pyros_ros/connections_list'),
('~diff', '/pyros_ros/connections_diff'),
])
try:
self.connection_cache_proc = self.launch.launch(
self.connection_cache_node)
except roslaunch.RLException as rlexc:
raise nose.SkipTest(
"Connection Cache Node not found (part of rocon_python_comms pkg). Skipping test."
)
node_api = None
with Timeout(5) as t:
while not t.timed_out and node_api is None:
node_api = rosnode.get_api_uri(rospy.get_master(),
'connection_cache')
assert node_api is not None # make sure the connection cache node is started before moving on.
super(TestPyrosROSCache, self).setUp(enable_cache=True)
def search_namespace(self):
cb = self.plugin_manager_widget.namespaceComboBox
cb.blockSignals(True)
cb.setEnabled(False)
cb.clear()
cb.addItem('Updating...')
# get topic list
_, _, topic_type = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_type)
# filter list
topic_dict_filtered = dict()
for k, v in topic_dict.items():
if v == 'vigir_pluginlib_msgs/GetPluginStatesActionGoal':
topic_dict_filtered[k] = v
# update combo box with found namespaces
cb.clear()
namespaces = [ns[:-37] for ns in sorted(topic_dict_filtered.keys())]
cb.addItems(namespaces)
if cb.count() > 0:
self.set_namespace(cb.currentText())
cb.setEnabled(True)
cb.blockSignals(False)
else:
cb.addItem('No topics available!')
def listener():
master = rospy.get_master()
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rate = rospy.Rate(rospy.get_param("~rate")) # Hz
rospy.Subscriber('chatter', String, callback, rate, queue_size=1)
# spin() simply keeps python from exiting until this node is stopped
#rospy.spin()
# Implementation of spin() with additional check that ros master exists
if not rospy.core.is_initialized():
raise rospy.exceptions.ROSInitException(
"client code must call rospy.init_node() first")
rospy.core.logdebug("node[%s, %s] entering spin(), pid[%s]",
rospy.core.get_caller_id(), rospy.core.get_node_uri(),
os.getpid())
try:
while not rospy.core.is_shutdown():
try:
pid = master.getPid()
except:
rospy.signal_shutdown('master killed')
else:
rospy.rostime.wallsleep(0.5)
except KeyboardInterrupt:
rospy.core.logdebug("keyboard interrupt, shutting down")
rospy.core.signal_shutdown('keyboard interrupt')
def discover_controllers(self):
rospy.logdebug("Discovering controllers")
# see http://wiki.ros.org/ROS/Master_API
master = rospy.get_master()
system_state = master.getSystemState()
cm_services = [
service for service, _provider in system_state[2][2]
if LIST_CONTROLLERS_SRV_NAME in service
]
for service in cm_services:
list_controllers = rospy.ServiceProxy(
service, controller_manager_msgs.srv.ListControllers)
try:
resp = list_controllers()
except rospy.ServiceException as exc:
rospy.logwarn("Service did not process request: " + str(exc))
else:
for controller in resp.controller:
if is_joint_trajectory_controller(
controller) and is_active(controller):
c = Controller()
ns = service[:service.find(LIST_CONTROLLERS_SRV_NAME)]
c.name = ns + "/" + controller.name
c.topic = c.name + "/command"
c.publisher = rospy.Publisher(
c.topic,
trajectory_msgs.msg.JointTrajectory,
queue_size=1)
c.joints = collect_joints(controller)
rospy.logdebug("Added controller '" + c.name + ".")
rospy.logdebug("--- topic: " + c.topic)
rospy.logdebug("--- joints: " + ",".join(c.joints))
self.controllers.append(c)
def update_topic_list(self):
self.topic_combo_box.clear()
self.topic_combo_box.setEnabled(False)
self.topic_combo_box.blockSignals(True)
self.topic_combo_box.addItem('Updating...')
# get topic list
_, _, topic_type = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_type)
# filter list
topic_dict_filtered = dict()
for k, v in topic_dict.items():
if (len(topic_type) == 0) or (v == self.topic_type):
if self.is_action_topic:
topic_dict_filtered[k[:-5]] = v
else:
topic_dict_filtered[k] = v
self.topic_combo_box.clear()
self.topic_combo_box.addItems(sorted(topic_dict_filtered.keys()))
if self.topic_combo_box.count() > 0:
self.topic_combo_box.setEnabled(True)
self.topic_combo_box.blockSignals(False)
self.topic_changed(self.topic_combo_box.currentText())
else:
self.topic_combo_box.addItem('No topics available!')
def isMasterAlive():
"""
return True if master alive and return False if
master is not alive
"""
global previous_run_id
try:
# first check the host is available
master = rospy.get_master()
master_host = re.search('http://([a-zA-Z0-9\-_]*):',
master.getUri()[2]).groups(1)[0]
response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null")
if response != 0:
print "master machine looks down"
return False
master.getSystemState()
run_id = rospy.get_param("/run_id")
if not previous_run_id:
previous_run_id = run_id
if run_id != previous_run_id:
print "run_id is not same"
previous_run_id = run_id
return False
return True
except Exception, e:
return False
def get_process_ros(node_name, doprint=False):
# get the node object from ros
node_api = rosnode.get_api_uri(rospy.get_master(),
node_name,
skip_cache=True)[2]
if not node_api:
rospy.logerr("could not get api of node %s (%s)" %
(node_name, node_api))
return False
# now try to get the Pid of this process
try:
response = ServerProxy(node_api).getPid('/NODEINFO')
except:
rospy.logerr("failed to get of the pid of ros node %s (%s)" %
(node_name, node_api))
return False
# try to get the process using psutil
try:
process = psutil.Process(response[2])
if doprint:
rospy.loginfo("adding new node monitor %s (pid %d)" %
(node_name, process.pid))
return process
except:
rospy.logerr("unable to open psutil object for %s" % (response[2]))
return False
def setup_pubsub():
"""
Test basic publisher registering/unregistering functionality
:return:
"""
setup()
topicname = '/test/pubsub_test'
master = rospy.get_master()
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname not in [p[0] for p in pubs])
assert_true(topicname not in [s[0] for s in subs])
# create the publisher
pub_test = rospy.Publisher(topicname, std_msgs.Empty, queue_size=1)
time.sleep(5) # leave it time
pubs, subs, services = master.getSystemState()[2]
print("Publishers :")
print([p[0] for p in pubs])
assert_true(topicname in [p[0] for p in pubs])
# create the subscriber
sub_test = rospy.Subscriber(topicname, std_msgs.Empty, fake_sub_cb)
time.sleep(2) # leave it time
pubs, subs, services = master.getSystemState()[2]
print("Subscribers:")
print([s[0] for s in subs])
assert_true(topicname in [s[0] for s in subs])
return topicname, pub_test, sub_test
def test_rospy_unregister_sub():
"""
Test basic subscriber registering/unregistering functionality
:return:
"""
setup()
topicname = '/test/sub_test'
master = rospy.get_master()
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname not in [s[0] for s in subs])
# create the publisher
sub_test = rospy.Subscriber(topicname, std_msgs.Empty, fake_sub_cb)
time.sleep(2) # leave it time
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname
in [s[0] for s in subs]) # FAIL ! Topic not registered
sub_test.unregister()
time.sleep(2) # leave it time
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname not in [s[0] for s in subs])
teardown()
def run(self):
try:
master = rospy.get_master()
master.getPid()
self.ros_master_present = True
except socket.error:
return
def _update_thread_run(self):
# update type_combo_box
message_type_names = []
try:
# this only works on fuerte and up
packages = sorted([
pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(
self._rospack, rosmsg.MODE_MSG)
])
except:
# this works up to electric
packages = sorted(rosmsg.list_packages())
for package in packages:
for base_type_str in rosmsg.list_msgs(package,
rospack=self._rospack):
message_class = roslib.message.get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
self.type_combo_box.setItems.emit(sorted(message_type_names))
# update topic_combo_box
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
def get_node_list():
"""
List all ROS Nodes
Return: List containing ROS Nodes(name, pid)
"""
rospy.logdebug("GET_NODE_LIST:")
node_array_temp = rosnode.get_node_names()
node_list = []
j = 0
for node_name in node_array_temp:
try:
node_api = rosnode.get_api_uri(rospy.get_master(), node_name)
if socket.gethostname() not in node_api[2] and socket.gethostbyname(socket.gethostname()) not in node_api[2]: # Only Get Info for Local Nodes
continue
code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID)
node_list.append(NODE(node_name, pid))
rospy.logdebug("Node_name: " + node_list[j].name + " Node_PID: " + str(node_list[j].pid))
j += 1
except socket_error as serr:
pass
rospy.logdebug("=============================")
return node_list
def get_exe_path(node_name):
ID = '/NODEINFO'
node_api = rosnode.get_api_uri(rospy.get_master(), node_name)
code, msg, pid = client.ServerProxy(node_api[2]).getPid(ID)
p = psutil.Process(pid)
#print(p.name())
return p.name()
def test_rospy_unregister_pub():
"""
Test basic publisher registering/unregistering functionality
:return:
"""
setup()
topicname = '/test/pub_test'
master = rospy.get_master()
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname not in [p[0] for p in pubs])
# create the publisher
pub_test = rospy.Publisher(topicname, std_msgs.Empty, queue_size=1)
time.sleep(2) # leave it time
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname in [p[0] for p in pubs])
pub_test.unregister()
time.sleep(2) # leave it time
pubs, subs, services = master.getSystemState()[2]
assert_true(topicname not in [p[0] for p in pubs])
teardown()
def search_namespace(self):
cb = self.plugin_manager_widget.namespaceComboBox
cb.blockSignals(True)
cb.setEnabled(False)
cb.clear()
cb.addItem('Updating...')
# get topic list
_, _, topic_type = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_type)
# filter list
topic_dict_filtered = dict()
for k, v in topic_dict.items():
if v == 'vigir_pluginlib_msgs/GetPluginStatesActionGoal':
topic_dict_filtered[k] = v
# update combo box with found namespaces
cb.clear()
namespaces = [ns[:-37] for ns in sorted(topic_dict_filtered.keys())]
cb.addItems(namespaces)
if cb.count() > 0:
self.set_namespace(cb.currentText())
cb.setEnabled(True)
cb.blockSignals(False)
else:
cb.addItem('No topics available!')
def setUp(self):
self.connection_cache_node = roslaunch.core.Node(
'rocon_python_comms',
'connection_cache.py',
name='connection_cache',
remap_args=[('~list', rospy.resolve_name('~connections_list')),
('~diff', rospy.resolve_name('~connections_diff'))])
# Easier to remap the node topic to the proxy ones, instead of the opposite, since there is no dynamic remapping.
# However for normal usecase, remapping the proxy handles is preferable.
try:
self.connection_cache_proc = TestRosInterface.launch.launch(
self.connection_cache_node)
except roslaunch.RLException as rlexc:
raise nose.SkipTest(
"Connection Cache Node not found (part of rocon_python_comms pkg). Skipping test."
)
assert self.connection_cache_proc.is_alive()
# wait for node to be started
node_api = None
with timeout(5) as t:
while not t.timed_out and node_api is None:
node_api = rosnode.get_api_uri(rospy.get_master(),
'connection_cache')
assert node_api is not None # make sure the connection cache node is started before moving on.
self.strpub = rospy.Publisher('/test/string', String, queue_size=1)
self.emppub = rospy.Publisher('/test/empty', Empty, queue_size=1)
self.interface = RosInterface(True)
def get_node_info(self, event):
"""
Get the list of all Nodes running on the host.
Update the __node_list
"""
if not self.__is_enabled:
pass
if self.__is_enabled:
nodes = []
for node_name in rosnode.get_node_names():
try:
node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True)
if self._id in node_api[2]:
nodes.append(node_name)
except :
pass
for node in nodes:
if node not in self.__node_list:
"""TODO currently not catching the exception here - master not running is a hard error so it does
not make sense to continue running.."""
node_api = rosnode.get_api_uri(rospy.get_master(), node, skip_cache=True)
try:
pid = self.get_node_pid(node_api, node)
if not pid:
continue
node_process = psutil.Process(pid)
new_node = NodeStatisticsHandler(
self._id, node, node_process)
self.__dict_lock.acquire(True)
self.__node_list[node] = new_node
self.__dict_lock.release()
except psutil.NoSuchProcess:
rospy.loginfo('pid of node %s could not be fetched' % node)
continue
self.__dict_lock.acquire()
to_remove = []
for node_name in self.__node_list:
if node_name not in nodes:
rospy.logdebug('removing %s from host' % node_name)
to_remove.append(node_name)
for node_name in to_remove:
self.remove_node(node_name)
self.__dict_lock.release()
def init_node(name, start_master=False):
""" Register a client node with the master.
Parameters
----------
name: str
Name of the node.
start_master: bool, default False
If True, start a ROS master if one isn't already running.
Returns
-------
master: ROSLaunchParent or ROSMasterStub instance
If a ROS master was started by this method, returns a
``ROSLaunchParent`` instance that can be used to shut down the master
with its ``shutdown()`` method. Otherwise, a ``ROSMasterStub`` is
returned that shows a warning when its ``shutdown()`` method is called.
"""
import roslaunch
import rospy
class ROSMasterStub:
@staticmethod
def shutdown():
warnings.warn(
"ROS master was started somewhere else and cannot be shut "
"down.")
try:
rospy.get_master().getPid()
except ConnectionRefusedError:
if start_master:
master = roslaunch.parent.ROSLaunchParent("master", [],
is_core=True)
master.start()
# make sure master is shut down on exit
atexit.register(master.shutdown)
else:
raise RuntimeError("ROS master is not running.")
else:
master = ROSMasterStub()
rospy.init_node(name)
return master
def setupUi(self):
self._ui.setupUi(self)
_, _, topic_types = rospy.get_master().getTopicTypes()
cmd_vel_topics = [topic[0] for topic in topic_types if topic[1] == "geometry_msgs/Twist"]
self._ui.cmd_vel_topic_combo_box.setItems.emit(sorted(cmd_vel_topics))
# self.cmd_vel_publisher = rospy.Publisher(str(self.cmd_vel_topic_combo_box.currentText()), Twist)
odom_topics = [topic[0] for topic in topic_types if topic[1] == "nav_msgs/Odometry"]
self._ui.odom_topic_combo_box.setItems.emit(sorted(odom_topics))
def _check_for_master(self):
# check if master is available
try:
rospy.get_master().getSystemState()
return
except Exception:
pass
# spawn thread to detect when master becomes available
self._wait_for_master_thread = threading.Thread(target=self._wait_for_master)
self._wait_for_master_thread.start()
self._wait_for_master_dialog = QMessageBox(QMessageBox.Question, self.tr('Waiting for ROS master'), self.tr("Could not find ROS master. Either start a 'roscore' or abort loading the plugin."), QMessageBox.Abort)
self._master_found_signal.connect(self._wait_for_master_dialog.done, Qt.QueuedConnection)
button = self._wait_for_master_dialog.exec_()
# check if master existence was not detected by background thread
no_master = button != QMessageBox.Ok
self._wait_for_master_dialog.deleteLater()
self._wait_for_master_dialog = None
if no_master:
raise PluginLoadError('RosPyPluginProvider._init_node() could not find ROS master')
def _update_thread_run(self):
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
if i.endswith("get_blocks_action/goal"):
namespaces.append(i[0:i.index("get_blocks_action/goal")])
self._widget.namespace_input.setItems.emit(namespaces)
def setUp(self):
# Wait for topics
self.topic_dict = {}
while '/test_synergy/grasp_synergy' not in self.topic_dict:
_, _, topic_types = rospy.get_master().getTopicTypes()
self.topic_dict = dict(topic_types)
print self.topic_dict
rospy.sleep(0.02)
rospy.sleep(0.1)
def setupUi(self):
self._ui.setupUi(self)
_, _, topic_types = rospy.get_master().getTopicTypes()
cmd_vel_topics = [ topic[0] for topic in topic_types if topic[1] == 'geometry_msgs/Twist' ]
self._ui.cmd_vel_topic_combo_box.setItems.emit(sorted(cmd_vel_topics))
#self.cmd_vel_publisher = rospy.Publisher(str(self.cmd_vel_topic_combo_box.currentText()), Twist)
odom_topics = [ topic[0] for topic in topic_types if topic[1] == 'nav_msgs/Odometry' ]
self._ui.odom_topic_combo_box.setItems.emit(sorted(odom_topics))
#self.odom_subscriber = rospy.Subscriber(str(self.odom_topic_combo_box.currentText()), Odometry, self.odometry_callback)
core_sensor_topics = [ topic[0] for topic in topic_types if topic[1] == 'kobuki_msgs/SensorState' ]
self._ui.core_topic_combo_box.setItems.emit(sorted(core_sensor_topics))
def isMasterAlive():
"""
return True if master alive and return False if
master is not alive
"""
try:
master = rospy.get_master()
master.getSystemState()
return True
except:
return False
def _update_thread_run(self):
"""Update the list of namespaces."""
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
print i
if i.endswith("smach/container_status"):
namespaces.append(i[0:i.index("smach/container_status")])
self._widget.namespace_input.setItems.emit(namespaces)
def get_node_info(self, node_name):
node_api = rosnode.get_api_uri(rospy.get_master(), node_name)
code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID)
if node_name in self.nodes:
return self.nodes[node_name]
else:
try:
p = psutil.Process(pid)
self.nodes[node_name] = p
return p
except:
return False
def __init__(self, node_name): super(ProcessInfoDisplay, self).__init__() self.node_name = node_name self.node_api = rosnode.get_api_uri(rospy.get_master(), node_name) code, msg, self.pid = xmlrpclib.ServerProxy(self.node_api[2]).getPid(ID) try: self.process = psutil.Process(self.pid) except: print 'Bad PID: %s, name: %s, code: %s, msg: %s' % (self.pid, self.node_name, code, msg) self.initUI() self.timer = QtCore.QTimer(self.parent()) self.timer.setInterval(1000) self.timer.timeout.connect(self.update_live_info) self.timer.start()
def main():
format = argparse.RawTextHelpFormatter
parser = argparse.ArgumentParser(formatter_class=format)
parser.add_argument(
"-p", "--proceed", action="store_true", help="Continue testing after a failed test until all tests complete"
)
parser.add_argument("-t", "--test", help=test_help())
args = parser.parse_args(rospy.myargv()[1:])
test_dict = {
"version": None,
"valid_tests": {
"0.7.0": ["Enable", "Messages", "Services", "Head", "MoveArms", "Grippers", "BlinkLEDs", "Cameras"]
},
}
test_dict["version"] = get_version()
if not test_dict["version"] in test_dict["valid_tests"].keys():
print ("Exiting: No tests specified for your software version: %s" % (test_dict["version"]))
return 1
try:
raw_input("Press <Enter> to Begin Smoke Test\n")
except Exception:
print ("\nExiting.")
return 1
hostname = re.split("http://|.local", rospy.get_master().getUri()[2])[1]
cur_time = time.localtime()
filename = "%s-%s.%s.%s-rsdk-%s.smoketest" % (
hostname,
cur_time.tm_mon,
cur_time.tm_mday,
cur_time.tm_year,
test_dict["version"],
)
if args.test == None:
print "Performing All Tests"
ros_init()
for t in test_dict["valid_tests"][test_dict["version"]]:
run_test(t, filename, args.proceed)
elif args.test in test_dict["valid_tests"][test_dict["version"]]:
ros_init()
run_test(args.test, filename, args.proceed)
else:
print ("Exiting: Invalid test provided: %s for %s version software" % (args.test, test_dict["version"]))
parser.print_help()
return 0
def __init__(self, namespace, aggregation_topic, topic_blacklist=['rosout', 'rosout_agg', 'Aggregation', 'Aggregated'], topic_whitelist=[], check_rate=1):
self.namespace = namespace
self.aggregation_topic = aggregation_topic
self.imported_packages = ["std_msgs", "teleop_msgs"]
self.available_topics = {}
self.blacklisted_topics = topic_blacklist
self.whitelisted_topics = topic_whitelist
self.topic_handlers = {}
self.update_publisher = rospy.Publisher(aggregation_topic, SerializedMessage)
self.master = rospy.get_master()
rate = rospy.Rate(check_rate)
rospy.loginfo("Loaded topic manager [TX]")
while not rospy.is_shutdown():
self.update_available_topics()
rate.sleep()
def isMasterAlive():
"""
return True if master alive and return False if
master is not alive
"""
try:
master = rospy.get_master()
master_host = re.search('http://([a-zA-Z0-9\-_]*):', master.getUri()[2]).groups(1)[0]
response = os.system("ping -W 10 -c 1 " + master_host + " > /dev/null")
if response != 0:
return False
master.getSystemState()
return True
except:
return False
def get_node_info(self, node_name, skip_cache=False):
node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=skip_cache)
try:
code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID)
if node_name in self.nodes:
return self.nodes[node_name]
else:
try:
p = psutil.Process(pid)
self.nodes[node_name] = p
return p
except:
return False
except xmlrpclib.socket.error:
if not skip_cache:
return self.get_node_info(node_name, skip_cache=True)
else:
return False
def connect_to_ros(self, init_timeout=3):
if self.connected_to_ros:
return True
# Attempt to connect to master node
class InitNodeThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.setDaemon(True)
self.inited = False
self.init_cv = threading.Condition()
def run(self):
rospy.loginfo('Master found. Connecting...')
rospy.init_node('rxbag', anonymous=True, disable_signals=True)
with self.init_cv:
self.inited = True
self.init_cv.notify_all()
try:
# Check whether ROS master is running
master = rospy.get_master()
master.getPid()
# If so (i.e. no exception), attempt to initialize node
init_thread = InitNodeThread()
with init_thread.init_cv:
init_thread.start()
init_thread.init_cv.wait(init_timeout)
if init_thread.inited:
rospy.core.register_signals()
rospy.loginfo('Connected to ROS master.')
self.connected_to_ros = True
return True
else:
rospy.logerr('Couldn\'t connect to ROS master.')
except Exception:
rospy.loginfo('ROS master not found.')
return False
def _update_thread_run(self):
# update type_combo_box
message_type_names = []
try:
# this only works on fuerte and up
packages = sorted([pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(self._rospack, rosmsg.MODE_MSG)])
except:
# this works up to electric
packages = sorted(rosmsg.list_packages())
for package in packages:
for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack):
message_class = roslib.message.get_message_class(base_type_str)
if message_class is not None:
message_type_names.append(base_type_str)
self.type_combo_box.setItems.emit(sorted(message_type_names))
# update topic_combo_box
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys()))
def get_pid(name):
try:
pid = [p.pid for p in psutil.process_iter() if name in str(p.name)]
return pid[0]
except IndexError:
pass
try:
node_id = '/NODEINFO'
node_api = rosnode.get_api_uri(rospy.get_master(), name)
code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(node_id)
except IOError:
pass
else:
return pid
try:
return int(check_output(["pidof", "-s", name]))
except CalledProcessError:
pass
rospy.logerr("Node '" + name + "' is not running!")
return None
def __initRos(self):
'''
Initializes Ros environment including
* roscore
* ardrone_driver
* joy_node
* ar_toolkit
* own node
'''
# .............................................................................
rospy.loginfo('stage 1: check roscore ....')
# .............................................................................
roscoreRunning=False
try:
#check master
master = rospy.get_master()
master.getSystemState()
roscoreRunning=True
except:
dlg = wx.MessageDialog(self,
"Roscore is not running. Yudrone cannot work without roscore. Should it be executed by yudrone?",
"Roscore is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION)
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_YES:
# open roscore as subprocess
self.openRoscore()
try:
#check master again
master = rospy.get_master()
master.getSystemState()
roscoreRunning=True
except:
roscoreRunning=False
if roscoreRunning == False:
rospy.logfatal('No sroscore; shutting down')
self.Destroy()
rospy.loginfo('Roscore is running')
# load parameters
setDefaultParameter()
# .............................................................................
rospy.loginfo('stage 2: check network ....')
# .............................................................................
ARDRONE_IP = rospy.get_param('yudrone/ARDRONE_IP')
self.__ROSDIR = rospy.get_param('yudrone/ROSDIR')
networkConnected = os.system('ping ' + ARDRONE_IP + ' -c 4 -i 0.2 -w 3 > /dev/null')
if networkConnected != 0:
rospy.logerr('not connected to ardrone (IP: ' + ARDRONE_IP + ')')
dlg = wx.MessageDialog(self,
"There is no ardrone connected (at IP= " + ARDRONE_IP + ")",
"Network error", wx.OK|wx.ICON_ERROR)
result = dlg.ShowModal()
dlg.Destroy()
else:
rospy.loginfo('connected to ardrone (IP: ' + ARDRONE_IP + ')')
# .............................................................................
rospy.loginfo('stage 3: check ardrone_driver ...')
# .............................................................................
# check node
self.ardroneDriverRunnig = os.system('rosnode list | grep /ardrone > /dev/null')
if self.ardroneDriverRunnig != 0:
# no driver
dlg = wx.MessageDialog(self,
"Ardrone_driver is not running. Should it be executed by yudrone?",
"Ardrone_driver is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION)
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_YES:
# open roscore as subprocess
self.openDriver()
# check node again
self.ardroneDriverRunnig = os.system('rosnode list | grep /ardrone > /dev/null')
if self.ardroneDriverRunnig != 0:
rospy.logerr('ardrone_driver is not running')
else:
rospy.loginfo('ardrone_driver is running')
self.navdataCounter=0
# .............................................................................
rospy.loginfo('stage 4: check joy ....')
# .............................................................................
# check joy_node
joyNodeRunning = os.system('rosnode list | grep /joy_node > /dev/null')
if joyNodeRunning != 0:
# no joy_node
dlg = wx.MessageDialog(self,
"Joynode is not running. Should it be executed by yudrone?",
"Joynode is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION)
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_YES:
# open joy_node as subprocess
self.openJoyNode()
# check joynode again
joyNodeRunning = os.system('rosnode list | grep /joy_node > /dev/null')
if joyNodeRunning != 0:
rospy.logerr('joynode is not running')
else:
rospy.loginfo('joynode is running')
# check yudrone_joy
joypadCtrlRunning = os.system('rosnode list | grep /yudrone_joy > /dev/null')
if joypadCtrlRunning != 0:
# no joy_node
dlg = wx.MessageDialog(self,
"yudrone_joy is not running. Should it be executed by yudrone?",
"yudrone_joy is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION)
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_YES:
# open joy_node as subprocess
self.openJoyCtrl()
# check yudrone_joy again
joypadCtrlRunning = os.system('rosnode list | grep /yudrone_joy > /dev/null')
if joypadCtrlRunning != 0:
rospy.logerr('yudrone_joy is not running')
else:
rospy.loginfo('yudrone_joy is running')
# .............................................................................
rospy.loginfo('stage 5: check ar_toolkit ....')
# .............................................................................
arNodeRunning = os.system('rostopic list | grep /ar/image > /dev/null')
if arNodeRunning != 0:
# no ar_node
dlg = wx.MessageDialog(self,
"Ar_toolkit is not running. Should it be executed by yudrone?",
"Ar_toolkit is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION)
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_YES:
# open ar-node as subprocess
self.openArToolkit()
# check ar_node again
arNodeRunning = os.system('rostopic list | grep /ar/image > /dev/null')
if arNodeRunning != 0:
rospy.logerr('ar_node is not running')
self.__imageTopic = '/ardrone/image_raw'
else:
rospy.loginfo('ar_node is running')
self.__imageTopic = '/ar/image'
# .............................................................................
rospy.loginfo('stage 6: check watchdog ....')
# .............................................................................
wdNodeRunning = os.system('rosnode list | grep /yudrone_watchdog > /dev/null')
if wdNodeRunning != 0:
# no watchdog
dlg = wx.MessageDialog(self,
"Watchdog is not running. Should it be executed by yudrone?",
"Watchdog is not running!", wx.YES_NO|wx.YES_DEFAULT|wx.ICON_QUESTION)
result = dlg.ShowModal()
dlg.Destroy()
if result == wx.ID_YES:
# open watchdog as subprocess
self.openWatchdog()
# check ar_node again
wdNodeRunning = os.system('rostopic list | grep /ar/image > /dev/null')
if wdNodeRunning != 0:
rospy.logerr('watchdog is not running')
else:
rospy.loginfo('watchdog is running')
# .............................................................................
rospy.loginfo('stage 7: init own node ...')
# .............................................................................
#create node
rospy.init_node('yudrone_flight')
#subscribers
self.sub_nav = rospy.Subscriber( "ardrone/navdata", Navdata, self.handle_navdata )
self.sub_tags = rospy.Subscriber( "tags", Tags, self.handle_tags ) # obtain tags (only for screen output)
self.sub_yaw = rospy.Subscriber( "/cmd_vel", Twist, self.handle_twist ) # obtain twist msgs (only for screen output)
self.sub_image = None
rospy.sleep(0.1)
#variables
self.camState='front' # initially the camera is set to front on ardrone
#stage 7: finalize ............................................
self.OnRBVideo()
self.currYaw = Twist()
rospy.loginfo('ROS initialized')
test_dict['version'] = get_version()
if not test_dict['version'] in test_dict['valid_tests'].keys():
print("Exiting: No tests specified for your software version: %s" % \
(test_dict['version'])
)
sys.exit(1)
try:
raw_input("Press <Enter> to Begin Smoke Test\n")
except:
print("\nExiting.")
sys.exit(1)
master_hostname = re.split(
'http://|.local',
rospy.get_master().getUri()[2]
)[1]
cur_time = time.localtime()
filename = (
"%s-%s.%s.%s-rsdk-%s.smoketest" % \
(master_hostname, cur_time.tm_mon, cur_time.tm_mday, \
cur_time.tm_year, test_dict['version'],)
)
if args.test == None:
print 'Performing All Tests'
ros_init()
for t in test_dict['valid_tests'][test_dict['version']]:
run_test(t, filename, args.proceed)
elif args.test in test_dict['valid_tests'][test_dict['version']]:
ros_init()
run_test(args.test, filename, args.proceed)
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import numpy as np
import cv2
from spinn_wrapper.msg import Spike
import rwlock as lck
cap = cv2.VideoCapture("/fzi/ids/mateev/catkin_ws/src/spinn_wrapper/src/spinn_wrapper/360x240.mp4")
#Do this to make sure roscore starts before the script starts executing
failed = True
while failed == True:
try:
failed = False
rospy.get_master().getPid()
except:
failed = True
start_time = time.time()
sources = []
#import a random grayscale image for testing purposes, extract 5 different values
#img = cv2.imread('/fzi/ids/mateev/catkin_ws/src/spinn_wrapper/src/spinn_wrapper/img.jpg',0)
#scaled_img = cv2.resize(img,(100,100))
pos_frame = cap.get(cv2.cv.CV_CAP_PROP_POS_FRAMES)
fnumber = cap.get(cv2.cv.CV_CAP_PROP_FRAME_COUNT)
while not cap.isOpened():
cap = cv2.VideoCapture("/fzi/ids/mateev/catkin_ws/src/spinn_wrapper/src/spinn_wrapper/360x240.mp4")
def poll(self):
# Read the current system state from the ROS master
publishers, subscribers, _ = map(dict, rospy.get_master().getSystemState()[2])
node_names = set(rosnode.get_node_names())
topic_types = rospy.get_master().getPublishedTopics('')[2]
# Update the dictionary of topic types
for topic, type in topic_types:
if self.topic_types.get(topic, None) != type:
self.updates.set_topic_type(topic, type)
self.topic_types[topic] = type
# Create new nodes (needed for nodes without any pub/sub topics)
for name in node_names:
self.node(name)
# Link up published topics
for topic in publishers:
if topic in TOPIC_NAMES_TO_IGNORE:
continue
for name in publishers[topic]:
if name in NODE_NAMES_TO_IGNORE:
continue
node = self.node(name)
if node is not None:
node.output(topic)
node_names.add(name)
# Link up subscribed topics
for topic in subscribers:
if topic in TOPIC_NAMES_TO_IGNORE:
continue
for name in subscribers[topic]:
if name in NODE_NAMES_TO_IGNORE:
continue
node = self.node(name)
if node is not None:
node.input(topic)
node_names.add(name)
# Remove old node names
killed_names = self.names_to_stop_avoiding - node_names
self.names_to_stop_avoiding -= killed_names
self.names_to_avoid -= killed_names
# Remove old slots
for name, node in self.nodes.items() + self.owned_nodes.items():
for input in node.inputs.values():
if input.topic not in subscribers or node.name not in subscribers[input.topic]:
del node.inputs[input.topic]
for output in node.outputs.values():
if output.topic not in publishers or node.name not in publishers[output.topic]:
del node.outputs[output.topic]
# Remove old nodes
for name in self.nodes.keys():
if name not in node_names:
del self.nodes[name]
# Update owned nodes
for name in self.owned_nodes:
node = self.owned_nodes[name]
# Check on the process
if node.status == 'Starting...' and node.name in node_names:
node.status = ''
self.updates.update_owned_node(node)
node.poll()
# Remember all observed topics for when we restart this node in the future
if node.path not in self.db.topics:
self.db.topics[node.path] = set(), set()
published, subscribed = self.db.topics[node.path]
published |= set(topic for topic in node.outputs if node.outputs[topic].original_topic is None)
subscribed |= set(topic for topic in node.inputs if node.inputs[topic].original_topic is None)
# Save all observed topics
self.db.save()
# Keep polling killed child processes until they actually die
for process in list(self.killed_processes):
process.poll()
if process.returncode is not None:
self.killed_processes.remove(process)
# Update launch files
for launch_file in list(self.launch_files.values()):
launch_file.poll()
def wait_topics():
_, _, topic_types = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_types)
while "/allegroHand_test/joint_states" not in topic_dict and "/allegroHand_test/tf" not in topic_dict:
rospy.sleep(0.02)
