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           
Exemple #2
0
    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)
Exemple #7
0
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