def loadStats(self, map_name): #point_set=str(sys.argv[1]) #map_name=str(sys.argv[3]) msg_store = MessageStoreProxy() query_meta = {} query_meta["pointset"] = map_name print "" message_list = msg_store.query(NavStatistics._type, {}, query_meta) available = len(message_list) > 0 print available if available <= 0 : rospy.logerr("Desired pointset '"+map_name+"' not in datacentre") rospy.logerr("Available pointsets: "+str(available)) raise Exception("Can't find waypoints.") else : stats = [] edges_list = [] for i in message_list: edg = {} edge = i[0].origin+'_'+i[0].target if edge not in edges_list : edges_list.append(edge) dat = i[1]["inserted_at"]#datetime.strptime(i[0].date_started, "%A, %B %d, at %H:%M:%S hours") epo = dat.strftime('%s') #Thursday, May 15, at 12:13:00 hours val = {} val['epoch'] = epo val['status'] = i[0].status val['time'] = i[0].operation_time val['edge'] = edge edg["stat"] = val stats.append(edg) #stats = sorted(stats, key=itemgetter('edge')) stata =[] for l in edges_list: # print '\n' # print "statistics for edge:" # print l statb = [] for j in stats : if j['stat']['edge'] == l : statb.append(j["stat"]) statb = sorted(statb, key=itemgetter('epoch')) stata.append(statb) return stata
def __init__(self, soma_map, soma_conf, config_file=None): self.soma_map = soma_map self.soma_conf = soma_conf if config_file: self._config_file = config_file else: # default file rp = RosPack() path = rp.get_path('soma_objects') + '/config/' filename = 'default.json' self._config_file=path+filename self._soma_obj_ids = dict() self._soma_obj_msg = dict() self._interactive = True self._msg_store=MessageStoreProxy(collection="soma") self._server = InteractiveMarkerServer("soma") self._init_types() self._init_menu() self.load_objects() rospy.spin()
def __init__(self, name): rospy.loginfo("Starting %s", name) self._action_name = name rospy.loginfo("Creating action server.") self._as = actionlib.SimpleActionServer(self._action_name, patrol_snapshot.msg.PatrolSnapshotAction, execute_cb = self.executeCallback, auto_start = False) self._as.register_preempt_callback(self.preemptCallback) current_time = datetime.now() self.dt_text= current_time.strftime('%A, %B %d, at %H:%M hours') self.msg_store = MessageStoreProxy(collection='patrol_data') rospy.loginfo(" ...starting") self._as.start() rospy.loginfo(" ...done")
class MessageStoreMapServer: def __init__(self): self.msg_store = MessageStoreProxy() self.map_publisher = rospy.Publisher('/map', OccupancyGrid, latch=True) self.publish_map_serv = rospy.Service('/switch_map', SwitchMap, self.serve_map_srv) def serve_map_srv(self, req): return self.serve_map(req.map_name) def serve_map(self, name): try: map = self.msg_store.query_named(name, OccupancyGrid._type) if map is None: rospy.logwarn("No map found with name \"%s\"", name) return False else: self.map_publisher.publish(map) return True except rospy.ServiceException, e: print "Service call failed: %s"%e return False
#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import ros_datacentre_msgs.srv as dc_srv import ros_datacentre.util as dc_util from ros_datacentre.message_store import MessageStoreProxy from geometry_msgs.msg import Pose, Point, Quaternion import StringIO if __name__ == '__main__': rospy.init_node("example_message_store_client") msg_store = MessageStoreProxy() p = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) try: # insert a pose object with a name msg_store.insert_named("my favourite pose", p) # get it back with a name print msg_store.query_named("my favourite pose", Pose._type) # try to get it back with an incorrect name, so get None instead print msg_store.query_named("my favourite position", Pose._type) # get all poses print msg_store.query(Pose._type) # get all non-existant typed objects, so get an empty list back print msg_store.query("not my type")
class patrolSnap(): # Create feedback and result messages #_feedback = scitos_ptu_sweep.msg.PTUSweepFeedback() #_result = scitos_ptu_sweep.msg.PTUSweepResult() _result = patrol_snapshot.msg.PatrolSnapshotResult() def __init__(self, name): rospy.loginfo("Starting %s", name) self._action_name = name rospy.loginfo("Creating action server.") self._as = actionlib.SimpleActionServer(self._action_name, patrol_snapshot.msg.PatrolSnapshotAction, execute_cb = self.executeCallback, auto_start = False) self._as.register_preempt_callback(self.preemptCallback) current_time = datetime.now() self.dt_text= current_time.strftime('%A, %B %d, at %H:%M hours') self.msg_store = MessageStoreProxy(collection='patrol_data') rospy.loginfo(" ...starting") self._as.start() rospy.loginfo(" ...done") def executeCallback(self, goal): current_time = datetime.now() self.dt_text= current_time.strftime('%A, %B %d, at %H:%M hours') print "New snap requested" #Subscribers print self.dt_text self.waypoint = 'None' print "/current_node" nod_rec=True try: msg = rospy.wait_for_message('/current_node', String, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get current node") nod_rec=False if nod_rec: self.waypoint = msg.data meta = {} meta["action"] = 'patrol_snapshot' meta["waypoint"] = self.waypoint meta["time"] = self.dt_text self.msg_store.insert(msg, meta) print "/robot_pose" nod_rec=True try: msg = rospy.wait_for_message('/robot_pose', Pose, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get robot pose") nod_rec=False if nod_rec: self.msg_store.insert(msg,meta) print "/scan" nod_rec=True try: msg = rospy.wait_for_message('/scan', LaserScan, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get scan") nod_rec=False if nod_rec: self.msg_store.insert(msg,meta) print "/head_xtion/rgb/image_color" nod_rec=True try: msg = rospy.wait_for_message('/head_xtion/rgb/image_color', Image, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get image_color") nod_rec=False if nod_rec: self.msg_store.insert(msg,meta) print "/head_xtion/depth/image_rect" nod_rec=True try: msg = rospy.wait_for_message('/head_xtion/depth/image_rect_meters', Image, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get image_color") nod_rec=False if nod_rec: self.msg_store.insert(msg,meta) print '/head_xtion/depth/points' received = True try: msg = rospy.wait_for_message('/head_xtion/depth/points', PointCloud2, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get point cloud") received = False if received : self.msg_store.insert(msg,meta) self._result.success = True self._as.set_succeeded(self._result) def preemptCallback(self): self._result.success = False self._as.set_preempted(self._result)
class SOMAManager(): def __init__(self, soma_map, soma_conf, config_file=None): self.soma_map = soma_map self.soma_conf = soma_conf if config_file: self._config_file = config_file else: # default file rp = RosPack() path = rp.get_path('soma_objects') + '/config/' filename = 'default.json' self._config_file=path+filename self._soma_obj_ids = dict() self._soma_obj_msg = dict() self._interactive = True self._msg_store=MessageStoreProxy(collection="soma") self._server = InteractiveMarkerServer("soma") self._init_types() self._init_menu() self.load_objects() rospy.spin() def _init_types(self): # read from config in soma_objects with open(self._config_file) as config_file: config = json.load(config_file) self.mesh = dict() for k, v in config['mesh'].iteritems(): self.mesh[k] = v def _init_menu(self): self.menu_handler = MenuHandler() add_entry = self.menu_handler.insert( "Add object" ) self.menu_item = dict() for i, k in enumerate(self.mesh): entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb) self.menu_item[entry] = k del_entry = self.menu_handler.insert( "Delete object", callback=self._del_cb) enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb ) self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED ) def _add_cb(self, feedback): rospy.loginfo("Add marker: %s", self.menu_item[feedback.menu_entry_id]) self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose) def _del_cb(self, feedback): rospy.loginfo("Delete marker: %s", feedback.marker_name) self.delete_object(feedback.marker_name) def _update_cb(self, feedback): p = feedback.pose.position print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) if hasattr(self, "vp_timer_"+feedback.marker_name): getattr(self, "vp_timer_"+feedback.marker_name).cancel() setattr(self, "vp_timer_"+feedback.marker_name, Timer(3, self.update_object, [feedback])) getattr(self, "vp_timer_"+feedback.marker_name).start() def _enable_cb(self, feedback): handle = feedback.menu_entry_id state = self.menu_handler.getCheckState( handle ) if state == MenuHandler.CHECKED: self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED ) self._interactive = False else: self.menu_handler.setCheckState( handle, MenuHandler.CHECKED ) self._interactive = True self.menu_handler.reApply( self._server ) self.load_objects() self._server.applyChanges() def _next_id(self): self._soma_id += 1 return self._soma_id def _retrieve_objects(self): objs = self._msg_store.query(SOMAObject._type, message_query={"map": self.soma_map, "config": self.soma_conf}) max_id = 0 for o,om in objs: if int(o.id) > max_id: max_id = int(o.id) self._soma_id = max_id return objs def load_objects(self): objs = self._retrieve_objects() # if collection is empty insert initial object if not objs: pose = Pose() self.add_object('Table', pose) return # otherwise, load all object from collection for o, om in objs: self._soma_obj_ids[o.id] = om['_id'] self._soma_obj_msg[o.id] = o self.load_object(o.id, o.type, o.pose) def load_object(self, soma_id, soma_type, pose): int_marker = self.create_object_marker(soma_id, soma_type, pose) self._server.insert(int_marker, self._update_cb) self.menu_handler.apply( self._server, soma_id ) self._server.applyChanges() def add_object(self, soma_type, pose): # todo: add to mongodb soma_id = self._next_id() soma_obj = SOMAObject() soma_obj.id = str(soma_id) soma_obj.map = str(self.soma_map) soma_obj.config = str(self.soma_conf) soma_obj.type = soma_type soma_obj.pose = pose soma_obj.frame = '/map' soma_obj.mesh = self.mesh[soma_type] _id = self._msg_store.insert(soma_obj) self._soma_obj_ids[soma_obj.id] = _id self._soma_obj_msg[soma_obj.id] = soma_obj self.load_object(str(soma_id), soma_type, pose) def delete_object(self, soma_id): # todo: delete from mongodb _id = self._soma_obj_ids[str(soma_id)] self._msg_store.delete(str(_id)) self._server.erase(soma_id) self._server.applyChanges() def update_object(self, feedback): print "Updated marker " + feedback.marker_name _id = self._soma_obj_ids[feedback.marker_name] msg = self._soma_obj_msg[feedback.marker_name] new_msg = copy.deepcopy(msg) new_msg.pose = feedback.pose self._msg_store.update_id(_id, new_msg) def create_object_marker(self, soma_obj, soma_type, pose): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = soma_obj int_marker.description = "id" + soma_obj int_marker.pose = pose mesh_marker = Marker() mesh_marker.type = Marker.MESH_RESOURCE mesh_marker.scale.x = 1 mesh_marker.scale.y = 1 mesh_marker.scale.z = 1 random.seed(soma_type) val = random.random() mesh_marker.color.r = r_func(val) mesh_marker.color.g = g_func(val) mesh_marker.color.b = b_func(val) mesh_marker.color.a = 1.0 #mesh_marker.pose = pose mesh_marker.mesh_resource = self.mesh[soma_type] # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE if self._interactive: int_marker.controls.append(copy.deepcopy(control)) # add the control to the interactive marker int_marker.controls.append(control); # add menu control menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True menu_control.markers.append( mesh_marker) #makeBox(int_marker) ) int_marker.controls.append(menu_control) return int_marker
def __init__(self): self.msg_store = MessageStoreProxy() self.map_publisher = rospy.Publisher('/map', OccupancyGrid, latch=True) self.publish_map_serv = rospy.Service('/switch_map', SwitchMap, self.serve_map_srv)
#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import ros_datacentre_msgs.srv as dc_srv import ros_datacentre.util as dc_util from ros_datacentre.message_store import MessageStoreProxy from geometry_msgs.msg import Pose, Point, Quaternion import StringIO if __name__ == '__main__': rospy.init_node("example_message_store_client") msg_store = MessageStoreProxy() p = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) try: # insert a pose object with a name msg_store.insert_named("my favourite pose", p) # get it back with a name print msg_store.query_named("my favourite pose", Pose._type) # try to get it back with an incorrect name, so get None instead print msg_store.query_named("my favourite position", Pose._type) # get all poses print msg_store.query(Pose._type) # get all non-existant typed objects, so get an empty list back