00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 from __future__ import division
00012 import os
00013 import math
00014 import threading
00015
00016 from python_qt_binding import loadUi
00017 from python_qt_binding.QtGui import QWidget
00018
00019 import rospkg
00020 import rospy
00021 from rocon_qt_library.widgets import QResourceChooser, QVideoTeleop, QSlamWidget
00022 from rocon_qt_library.interfaces import ResourceChooserInterface, SlamWidgetInterface
00023
00024 from qt_gui.plugin import Plugin
00025
00026
00027
00028
00029
00030
00031 class MakeAMap(Plugin):
00032
00033 def __init__(self, context):
00034 self._lock = threading.Lock()
00035 self._context = context
00036 super(MakeAMap, self).__init__(context)
00037
00038 self.initialised = False
00039 self.is_setting_dlg_live = False
00040
00041 self._widget = QWidget()
00042 rospack = rospkg.RosPack()
00043 ui_file = os.path.join(rospack.get_path('concert_qt_make_a_map'), 'ui', 'concert_make_a_map.ui')
00044 loadUi(ui_file, self._widget, {'QResourceChooser': QResourceChooser, 'QVideoTeleop': QVideoTeleop, 'QSlamWidget': QSlamWidget})
00045 if context.serial_number() > 1:
00046 self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00047
00048
00049 self._set_resource_chooser_interface()
00050
00051 context.add_widget(self._widget)
00052 self.setObjectName('Make a Map')
00053
00054 self._default_cmd_vel_topic = '/teleop/cmd_vel'
00055 self._default_compressed_image_topic = '/teleop/compressed_image'
00056 self._default_map_topic = 'map'
00057 self._default_scan_topic = '/make_a_map/scan'
00058 self._default_robot_pose = 'robot_pose'
00059 self._default_wc_namespace_param = 'wc_namespace'
00060 self._default_wc_namespace = 'world_canvas'
00061
00062 def _set_resource_chooser_interface(self):
00063 capture_timeout = rospy.get_param('~capture_timeout', 15.0)
00064 service_name = rospy.get_param('~service_name')
00065
00066
00067 available_resource_topic = '/services/%s/available_make_a_map'%service_name
00068 capture_resource_pair_topic = '/services/%s/capture_make_a_map'%service_name
00069 capture_resource_callbacks = [self._widget.resource_chooser_widget.capture_resource_callback, self._init_teleop_interface, self._set_slam_view_interface]
00070 release_resource_callbacks = [self._widget.resource_chooser_widget.release_resource_callback, self._uninit_teleop_interface]
00071 error_resource_callbacks = [self._widget.resource_chooser_widget.error_resource_callback]
00072 refresh_resource_list_callbacks = [self._widget.resource_chooser_widget.refresh_resource_list_callback]
00073
00074 self._resource_chooser_interface = ResourceChooserInterface(capture_timeout, available_resource_topic, capture_resource_pair_topic, capture_resource_callbacks, release_resource_callbacks, error_resource_callbacks, refresh_resource_list_callbacks)
00075
00076 capture_event_callbacks = [self._resource_chooser_interface.capture_resource]
00077 release_event_callbacks = [self._resource_chooser_interface.release_resource, self._unset_slam_interface]
00078 self._widget.resource_chooser_widget.set_callbacks(capture_event_callbacks, release_event_callbacks)
00079
00080 def _init_teleop_interface(self, uri, msg):
00081 if msg.result:
00082 cmd_vel_topic = self._get_remaps(self._default_cmd_vel_topic, msg.remappings)
00083 compressed_image_topic = self._get_remaps(self._default_compressed_image_topic, msg.remappings)
00084
00085 with self._lock:
00086 self._widget.video_teleop_widget.init_teleop_interface(cmd_vel_topic_name=cmd_vel_topic, compressed_image_topic_name=compressed_image_topic)
00087
00088 def _get_remaps(self, remap_from, remappings):
00089 for r in remappings:
00090 if r.remap_from == remap_from:
00091 return r.remap_to
00092 return remap_from
00093
00094 def _uninit_teleop_interface(self, uri, msg):
00095 if msg.result:
00096 with self._lock:
00097 self._widget.video_teleop_widget.reset()
00098
00099 def _unset_slam_interface(self, uri):
00100 self._widget.slam_widget.unset_slam_interface()
00101
00102 def _set_slam_view_interface(self, uri, msg):
00103 if msg.result:
00104 scan_slot = self._widget.slam_widget.draw_scan
00105 robot_pose_slot = self._widget.slam_widget.draw_robot_pose
00106
00107 wc_namespace_param = rospy.get_param('~wc_namespace_param')
00108 wc_namespace = rospy.get_param(wc_namespace_param, self._default_wc_namespace)
00109 map_topic = self._get_remaps(self._default_map_topic , msg.remappings)
00110 scan_topic = self._get_remaps(self._default_scan_topic, msg.remappings)
00111 robot_pose_topic = self._get_remaps(self._default_robot_pose, msg.remappings)
00112 map_saved_callbacks = [self._widget.slam_widget.map_saved_callback]
00113 self._widget.slam_widget.init_slam_widget_interface(map_topic=map_topic,
00114 scan_received_slot=scan_slot,
00115 scan_topic=scan_topic,
00116 robot_pose_received_slot=robot_pose_slot,
00117 robot_pose_topic=robot_pose_topic,
00118 wc_namespace=wc_namespace,
00119 map_saved_callbacks=map_saved_callbacks)
00120
00121 def shutdown_plugin(self):
00122 self._widget.slam_widget.unset_slam_interface()
00123 self._widget.video_teleop_widget.reset()
00124 self._widget.resource_chooser_widget.shutdown()