class GraspViewerGUI(Plugin):
"""
a rqtgui plugin for the grasp viewer
"""
def __init__(self, context):
Plugin.__init__(self, context)
self.setObjectName('RqtGraspViewer')
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_grasp_viewer'),
'ui', 'RqtGraspViewer.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RqtGraspViewerUi')
context.add_widget(self._widget)
main_layout = QHBoxLayout()
self._default_labels = [
"obj_id", "object", "grasp_id", "grasp", "quality"
]
self._filemodel = QStandardItemModel(0, 5)
self._filemodel.setHorizontalHeaderLabels(self._default_labels)
self._table_view = QTableView()
self._table_view.setModel(self._filemodel)
self._table_view.resizeColumnsToContents()
main_layout.addWidget(self._table_view)
self._widget.scrollarea.setLayout(main_layout)
self.init_services()
self.init_subscribers()
# self._table_view.clicked.connect(self.on_table_view_clicked)
self._table_view.selectionModel().selectionChanged.connect(
self.on_table_view_select)
#self._widget.treeWidget.itemClicked.connect(self.on_posture_clicked)
def init_services(self):
"""
Init services
"""
service_name = '/display_grasp'
try:
rospy.wait_for_service(service_name)
self._grasp_viz_client = rospy.ServiceProxy(
service_name, DisplayGrasps)
rospy.loginfo("Found %s", service_name)
except rospy.ROSException:
rospy.logerr("%s did not show up. Giving up", service_name)
def init_subscribers(self):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._grasp_sub = rospy.Subscriber("/grasp_manager/result",
GraspPlanningActionResult,
self.graspable_result_cb)
def graspable_result_cb(self, msg):
result = msg.result
if len(result.grasps) > 0:
fm = self._filemodel
fm.clear()
fm.setHorizontalHeaderLabels(self._default_labels)
self.grasps = result.grasps
self.populate_table()
def populate_table(self):
"""
update table
"""
fm = self._filemodel
i = 0
for j, grasp in enumerate(self.grasps):
item_id = QStandardItem(str(i))
item_id.setEditable(False)
item_obj = QStandardItem("unknown")
item_obj.setEditable(False)
idem_grasp_id = QStandardItem(str(j))
idem_grasp_id.setEditable(False)
item_grasp = QStandardItem(grasp.id)
item_grasp.setEditable(False)
item_grasp_quality = QStandardItem(str(grasp.grasp_quality))
item_grasp_quality.setEditable(False)
fm.appendRow([
item_id, item_obj, idem_grasp_id, item_grasp,
item_grasp_quality
])
self._table_view.resizeColumnsToContents()
#fm = self._filemodel[group_name]
#item_idx = fm.index(idx, 0)
#fm.setData(item_idx, str(time_from_start))
def on_table_view_clicked(self, index):
items = self._table_view.selectedIndexes()
for it in items:
print 'selected item index found at %s' % it.row()
def on_table_view_select(self, selected, deselected):
fm = self._filemodel
items = self._table_view.selectedIndexes()
if len(items) > 0:
if len(items) > 5:
rospy.logwarn("More than 5 items selected, \
but grasp viewer can only display 5 grasps at the same time"
)
req = DisplayGraspsRequest()
for it in items:
row = it.row()
obj_idx = fm.index(it.row(), 0)
obj_id = int(fm.data(obj_idx))
grasp_idx = fm.index(it.row(), 2)
grasp_id = int(fm.data(grasp_idx))
req.grasps.append(self.grasps[grasp_id])
#print "selected item index found at ", obj_id, grasp_id
self._grasp_viz_client(req)
#########
# Default methods for the rqtgui plugins
def save_settings(self, global_settings, perspective_settings):
pass
def restore_settings(self, global_settings, perspective_settings):
pass
class HipViewerPlugin(QObject):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(HipViewerPlugin, self).__init__(context)
self.setObjectName('HipViewer')
self._current_dotcode = None
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'hip_viewer.ui')
loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('HipViewerUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'), -1)
self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
self.connect(self, SIGNAL('update_planning_graph'), self.update_planning_graph_synchronous)
# start the planner in a separate thread
planning_node = HipViewerNode(ex_callback = self.update_planning_graph)
planning_node.start()
def save_settings(self, global_settings, perspective_settings):
perspective_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex())
perspective_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text())
perspective_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked())
perspective_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked())
perspective_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, global_settings, perspective_settings):
self._widget.graph_type_combo_box.setCurrentIndex(int(perspective_settings.value('graph_type_combo_box_index', 0)))
self._widget.filter_line_edit.setText(perspective_settings.value('filter_line_edit_text', ''))
self._widget.quiet_check_box.setChecked(perspective_settings.value('quiet_check_box_state', True) in [True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(perspective_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(perspective_settings.value('highlight_connections_check_box_state', True) in [True, 'true'])
def update_planning_graph(self, op, htree):
self.emit(SIGNAL('update_planning_graph'), op, htree)
def update_planning_graph_synchronous(self, op, htree):
graph = hip.dot_from_plan_tree(htree)
graph.layout('dot')
self._update_graph_view(graph)
def _update_graph_view(self, graph):
self._graph = graph
self._redraw_graph_view()
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)
return url
class HipViewerPlugin(QObject):
_deferred_fit_in_view = Signal()
def __init__(self, context):
super(HipViewerPlugin, self).__init__(context)
self.setObjectName('HipViewer')
self._current_dotcode = None
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'hip_viewer.ui')
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self._widget.setObjectName('HipViewerUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
self._scene = QGraphicsScene()
self._widget.graphics_view.setScene(self._scene)
self._widget.graph_type_combo_box.insertItem(0, self.tr('infinite'),
-1)
self._widget.graph_type_combo_box.insertItem(1, self.tr('1'), 2)
self._widget.graph_type_combo_box.insertItem(2, self.tr('2'), 3)
self._widget.graph_type_combo_box.insertItem(3, self.tr('3'), 4)
self._widget.graph_type_combo_box.insertItem(4, self.tr('4'), 5)
self._widget.graph_type_combo_box.setCurrentIndex(0)
self._widget.refresh_graph_push_button.setIcon(
QIcon.fromTheme('view-refresh'))
self._widget.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.auto_fit_graph_check_box.toggled.connect(
self._redraw_graph_view)
self._widget.fit_in_view_push_button.setIcon(
QIcon.fromTheme('zoom-original'))
self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view)
self._widget.load_dot_push_button.setIcon(
QIcon.fromTheme('document-open'))
self._widget.load_dot_push_button.pressed.connect(self._load_dot)
self._widget.save_dot_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_dot_push_button.pressed.connect(self._save_dot)
self._widget.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self._widget.save_as_svg_push_button.pressed.connect(self._save_svg)
self._widget.save_as_image_push_button.setIcon(
QIcon.fromTheme('image'))
self._widget.save_as_image_push_button.pressed.connect(
self._save_image)
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
context.add_widget(self._widget)
self.connect(self, SIGNAL('update_planning_graph'),
self.update_planning_graph_synchronous)
# start the planner in a separate thread
planning_node = HipViewerNode(ex_callback=self.update_planning_graph)
planning_node.start()
def save_settings(self, global_settings, perspective_settings):
perspective_settings.set_value(
'graph_type_combo_box_index',
self._widget.graph_type_combo_box.currentIndex())
perspective_settings.set_value('filter_line_edit_text',
self._widget.filter_line_edit.text())
perspective_settings.set_value(
'quiet_check_box_state', self._widget.quiet_check_box.isChecked())
perspective_settings.set_value(
'auto_fit_graph_check_box_state',
self._widget.auto_fit_graph_check_box.isChecked())
perspective_settings.set_value(
'highlight_connections_check_box_state',
self._widget.highlight_connections_check_box.isChecked())
def restore_settings(self, global_settings, perspective_settings):
self._widget.graph_type_combo_box.setCurrentIndex(
int(perspective_settings.value('graph_type_combo_box_index', 0)))
self._widget.filter_line_edit.setText(
perspective_settings.value('filter_line_edit_text', ''))
self._widget.quiet_check_box.setChecked(
perspective_settings.value('quiet_check_box_state', True) in
[True, 'true'])
self._widget.auto_fit_graph_check_box.setChecked(
perspective_settings.value('auto_fit_graph_check_box_state', True)
in [True, 'true'])
self._widget.highlight_connections_check_box.setChecked(
perspective_settings.value('highlight_connections_check_box_state',
True) in [True, 'true'])
def update_planning_graph(self, op, htree):
self.emit(SIGNAL('update_planning_graph'), op, htree)
def update_planning_graph_synchronous(self, op, htree):
graph = hip.dot_from_plan_tree(htree)
graph.layout('dot')
self._update_graph_view(graph)
def _update_graph_view(self, graph):
self._graph = graph
self._redraw_graph_view()
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)
return url
class GraspViewerGUI(Plugin):
"""
a rqtgui plugin for the grasp viewer
"""
def __init__(self, context):
Plugin.__init__(self, context)
self.setObjectName('RqtGraspViewer')
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_grasp_viewer'), 'ui', 'RqtGraspViewer.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RqtGraspViewerUi')
context.add_widget(self._widget)
main_layout = QHBoxLayout()
self._default_labels = ["obj_id", "object", "grasp_id", "grasp", "quality"]
self._filemodel = QStandardItemModel(0, 5)
self._filemodel.setHorizontalHeaderLabels(self._default_labels)
self._table_view = QTableView()
self._table_view.setModel(self._filemodel)
self._table_view.resizeColumnsToContents()
main_layout.addWidget(self._table_view)
self._widget.scrollarea.setLayout(main_layout)
self.init_services()
self.init_subscribers()
# self._table_view.clicked.connect(self.on_table_view_clicked)
self._table_view.selectionModel().selectionChanged.connect(self.on_table_view_select)
#self._widget.treeWidget.itemClicked.connect(self.on_posture_clicked)
def init_services(self):
"""
Init services
"""
service_name = '/display_grasp'
try:
rospy.wait_for_service(service_name)
self._grasp_viz_client = rospy.ServiceProxy(service_name, DisplayGrasps)
rospy.loginfo("Found %s", service_name)
except rospy.ROSException:
rospy.logerr("%s did not show up. Giving up", service_name)
def init_subscribers(self):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._grasp_sub = rospy.Subscriber("/grasp_manager/result", GraspPlanningActionResult,
self.graspable_result_cb)
def graspable_result_cb(self, msg):
result = msg.result
if len(result.grasps) > 0:
fm = self._filemodel
fm.clear()
fm.setHorizontalHeaderLabels(self._default_labels)
self.grasps = result.grasps
self.populate_table()
def populate_table(self):
"""
update table
"""
fm = self._filemodel
i = 0
for j, grasp in enumerate(self.grasps):
item_id = QStandardItem(str(i))
item_id.setEditable(False)
item_obj = QStandardItem("unknown")
item_obj.setEditable(False)
idem_grasp_id = QStandardItem(str(j))
idem_grasp_id.setEditable(False)
item_grasp = QStandardItem(grasp.id)
item_grasp.setEditable(False)
item_grasp_quality = QStandardItem(str(grasp.grasp_quality))
item_grasp_quality.setEditable(False)
fm.appendRow([item_id, item_obj, idem_grasp_id, item_grasp, item_grasp_quality])
self._table_view.resizeColumnsToContents()
#fm = self._filemodel[group_name]
#item_idx = fm.index(idx, 0)
#fm.setData(item_idx, str(time_from_start))
def on_table_view_clicked(self, index):
items = self._table_view.selectedIndexes()
for it in items:
print 'selected item index found at %s' % it.row()
def on_table_view_select(self, selected, deselected):
fm = self._filemodel
items = self._table_view.selectedIndexes()
if len(items) > 0:
if len(items) > 5:
rospy.logwarn("More than 5 items selected, \
but grasp viewer can only display 5 grasps at the same time")
req = DisplayGraspsRequest()
for it in items:
row = it.row()
obj_idx = fm.index(it.row(), 0)
obj_id = int(fm.data(obj_idx))
grasp_idx = fm.index(it.row(), 2)
grasp_id = int(fm.data(grasp_idx))
req.grasps.append(self.grasps[grasp_id])
#print "selected item index found at ", obj_id, grasp_id
self._grasp_viz_client(req)
#########
# Default methods for the rqtgui plugins
def save_settings(self, global_settings, perspective_settings):
pass
def restore_settings(self, global_settings, perspective_settings):
pass
class SrMoveObject(Plugin):
def __init__(self, context):
super(SrMoveObject, self).__init__(context)
self.setObjectName('SrMoveObject')
self._publisher = None
self._widget = QWidget()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../uis/SrMoveObjects.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('SrMoveObjectsUi')
context.add_widget(self._widget)
#attaching the button press event to their actions
self._widget.btnRefreshList.pressed.connect(self.on_refresh_list_clicked_)
self._widget.btnZero.pressed.connect(self.on_zero_clicked_)
self._widget.btnOpposite.pressed.connect(self.on_opposite_clicked_)
self._widget.btn_wrench.pressed.connect(self.on_apply_wrench_clicked_)
self._widget.btnSetToPos.pressed.connect(self.on_set_to_position_clicked_)
def on_zero_clicked_(self):
self._widget.lineEdit.setText("0.0")
self._widget.lineEdit_2.setText("0.0")
self._widget.lineEdit_3.setText("0.0")
def on_opposite_clicked_(self):
self._widget.lineEdit.setText( str((-1) * float(str(self._widget.lineEdit.text()))) )
self._widget.lineEdit_2.setText( str((-1) * float(str(self._widget.lineEdit_2.text()))) )
self._widget.lineEdit_3.setText( str((-1) * float(str(self._widget.lineEdit_3.text()))) )
def on_set_to_position_clicked_(self):
success = True
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
model_state = ModelState()
model_state.model_name = str(self._widget.comboBoxObjectList.currentText()).split('::')[0]
model_state.pose = Pose()
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.position.x = float(str(self._widget.lineEdit_4.text()))
model_state.pose.position.y = float(str(self._widget.lineEdit_5.text()))
model_state.pose.position.z = float(str(self._widget.lineEdit_6.text()))
try:
resp1 = set_model_state(model_state)
except rospy.ServiceException:
success = False
if success:
if not resp1.success:
success = False
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not set model state.")
def on_refresh_list_clicked_(self):
self._widget.comboBoxObjectList.clear()
success = True
get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
object_list = list()
try:
resp1 = get_world_properties()
except rospy.ServiceException:
success = False
if success:
get_model_properties = rospy.ServiceProxy('/gazebo/get_model_properties', GetModelProperties)
for model in resp1.model_names:
try:
model_properties = get_model_properties(model)
except rospy.ServiceException:
success = False
if success:
for body in model_properties.body_names:
object_list.append(model + '::' + body)
self._widget.comboBoxObjectList.addItems(object_list)
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not load object list from Gazebo.")
def on_apply_wrench_clicked_(self):
success = True
apply_body_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench)
body_name = self._widget.comboBoxObjectList.currentText()
wrench = Wrench()
wrench.force.x = float(str(self._widget.lineEdit.text()))
wrench.force.y = float(str(self._widget.lineEdit_2.text()))
wrench.force.z = float(str(self._widget.lineEdit_3.text()))
try:
resp1 = apply_body_wrench(body_name, "", None, wrench, rospy.Time.from_sec(0), rospy.Duration.from_sec(1.0))
except rospy.ServiceException:
success = False
if success:
if not resp1.success:
success = False
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not apply wrench to selected object.")
def _unregisterPublisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._unregisterPublisher()
def save_settings(self, global_settings, perspective_settings):
pass
def restore_settings(self, global_settings, perspective_settings):
pass
class SrGuiCountingDemo(Plugin):
def __init__(self, context):
super(SrGuiCountingDemo, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('SrGuiCountingDemo')
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../uis/sr_counting_demo.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('SrCountingDemoUi')
# Add widget to the user interface
context.add_widget(self._widget)
#Setting the initial state of the controller buttons
self._widget.btn_1.setEnabled(False)
self._widget.btn_2.setEnabled(False)
self._widget.btn_3.setEnabled(False)
self._widget.btn_4.setEnabled(False)
self._widget.btn_5.setEnabled(False)
# Attaching the button press event to their actions
self._widget.play.pressed.connect(self.start_demo)
self._widget.btn_1.pressed.connect(self.number_one_clicked)
self._widget.btn_2.pressed.connect(self.number_two_clicked)
self._widget.btn_3.pressed.connect(self.number_three_clicked)
self._widget.btn_4.pressed.connect(self.number_four_clicked)
self._widget.btn_5.pressed.connect(self.number_five_clicked)
def start_demo(self):
"""
This function allows to:
1. run the action server node
2. run the get_joint_state node
3. load some parameters
"""
newpid = os.fork()
if(newpid > 0):
os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui.launch")
os._exit(0)
self._widget.play.setEnabled(False)
self._widget.btn_1.setEnabled(True)
self._widget.btn_2.setEnabled(True)
self._widget.btn_3.setEnabled(True)
self._widget.btn_4.setEnabled(True)
self._widget.btn_5.setEnabled(True)
def number_one_clicked(self):
# send goal = 1 to the action server
newpid = os.fork()
if(newpid > 0):
os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=1")
os._exit(0)
def number_two_clicked(self):
# send goal = 2 to the action server
newpid = os.fork()
if(newpid > 0):
os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=2")
os._exit(0)
def number_three_clicked(self):
# send goal = 3 to the action server
newpid = os.fork()
if(newpid > 0):
os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=3")
os._exit(0)
def number_four_clicked(self):
# send goal = 4 to the action server
newpid = os.fork()
if(newpid > 0):
os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=4")
os._exit(0)
def number_five_clicked(self):
# send goal = 5 to the action server
newpid = os.fork()
if(newpid > 0):
os.system("roslaunch sr_counting_demo gazebo_sr_counting_demo_gui_client.launch goal:=5")
os._exit(0)
def _unregisterPublisher(self):
pass
def shutdown_plugin(self):
pass
