def gen_blog_entry(self, roi_id, uuid): print 'Region: ' + self.get_roi_name(roi_id) time = dt.fromtimestamp(int(rospy.get_time())) body = '### INTRUSION DETECTION REPORT\n\n' body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n' body += '- **Person UUID:** ' + str(uuid) + '\n\n' body += '- **Time:** ' + str(time) + '\n\n' #body += '- **Summary**: <font color="green">ALLOWED ITEMS (' + str(len(pos_objs)) + ')</font>, <font color="red">NOT-ALLOWED ITEMS (' + str(len(neg_objs)) + ')</font>\n\n' # # Create some blog entries msg_store = MessageStoreProxy(collection=self.blog_collection) robblog_path = roslib.packages.get_pkg_dir('soma_utils') img = rospy.wait_for_message('/upper_body_detector/image', Image, 5) bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="bgr8") ros_img = bridge.cv2_to_imgmsg(cv_image) img_id = msg_store.insert(ros_img) body += '<font color="red">Detected person:</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id e = RobblogEntry(title=str(time) + " Intrusion Detection - " + self.get_roi_name(roi_id), body=body) msg_store.insert(e)
def update_edge(self, node_name, edge_id, new_action=None, new_top_vel=None): msg_store = MessageStoreProxy(collection='topological_maps') # The query retrieves the node name with the given name from the given pointset. query = {"name": node_name, "pointset": self.name} # The meta-information is some additional information about the specific # map that we are interested in (?) query_meta = {} query_meta["pointset"] = self.name query_meta["map"] = self.map # This returns a tuple containing the object, if it exists, and some # information about how it's stored in the database. available = msg_store.query(TopologicalNode._type, query, query_meta) if len(available) == 1: for edge in available[0][0].edges: if edge.edge_id == edge_id: edge.action = new_action or edge.action edge.top_vel = new_top_vel or edge.top_vel msg_store.update(available[0][0], query_meta, query, upsert=True) else: rospy.logerr("Impossible to store in DB " + str(len(available)) + " waypoints found after query") rospy.logerr("Available data: " + str(available))
def __init__(self): self.task_msp = MessageStoreProxy(collection='mdp_task_exec_logs') self.nav_msp = MessageStoreProxy(collection='mdp_nav_exec_logs') self.current_goal_id = None self.active_task_stats = {} self.active_nav_stats = {} self.terminal_states = [ GoalStatus.PREEMPTED, GoalStatus.SUCCEEDED, GoalStatus.ABORTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.LOST ] mdp_goal_sub = rospy.Subscriber("mdp_plan_exec/execute_policy/goal", ExecutePolicyActionGoal, self.mdp_goal_cb) mdp_feedback_sub = rospy.Subscriber( "mdp_plan_exec/execute_policy/feedback", ExecutePolicyActionFeedback, self.mdp_feedback_cb) mdp_status_sub = rospy.Subscriber( "mdp_plan_exec/execute_policy/status", GoalStatusArray, self.mdp_status_cb) self.nav_stat_sub = rospy.Subscriber( "topological_navigation/Statistics", NavStatistics, self.nav_stats_cb) self.get_edge_estimates = rospy.ServiceProxy( "/topological_prediction/predict_edges", PredictEdgeState) rospy.loginfo("Logger started")
def remove_node(self, node_name) : rospy.loginfo('Removing Node: '+node_name) msg_store = MessageStoreProxy(collection='topological_maps') query = {"name" : node_name, "pointset": self.name} query_meta = {} query_meta["pointset"] = self.name query_meta["map"] = self.map available = msg_store.query(TopologicalNode._type, query, query_meta) node_found = False if len(available) == 1 : node_found = True rm_id = str(available[0][1]['_id']) print rm_id else : rospy.logerr("Node not found "+str(len(available))+" waypoints found after query") #rospy.logerr("Available data: "+str(available)) if node_found : query_meta = {} query_meta["pointset"] = self.name edges_to_rm = [] message_list = msg_store.query(TopologicalNode._type, {}, query_meta) for i in message_list: for j in i[0].edges : if j.node == node_name : edge_rm = i[0].name+'_'+node_name edges_to_rm.append(edge_rm) for k in edges_to_rm : print 'remove: '+k self.remove_edge(k) msg_store.delete(rm_id)
def get_maps(): """ Queries the database and returns details of the available topological maps. :return: A dictionary where each key is the name of a topological map and each item is a dictionary of details. Details are: "number_nodes" ; integer "edge_actions" : list of action server names used for traversal "last_modified" : datetime.datetime object for the last time a node was inserted """ maps = dict() msg_store = MessageStoreProxy(collection='topological_maps') nodes = msg_store.query(TopologicalNode._type) for node in nodes: pointset = node[1]["pointset"] if not maps.has_key(pointset): maps[pointset] = {"number_nodes": 0, "edge_actions": set(), "last_modified": ""} maps[pointset]["number_nodes"] += 1 if (maps[pointset]["last_modified"] == "" or node[1]["inserted_at"] > maps[pointset]["last_modified"]): maps[pointset]["last_modified"] = node[1]["inserted_at"] for edge in node[0].edges: maps[pointset]["edge_actions"].add(edge.action) return maps
def __init__(self, config_file=None): self._map = dict() self._obj = dict() self._roi = dict() self._roi_cnt = dict() self._obj_cnt = dict() self._num_of_objs = 0 self._num_of_rois = 0 self._soma_utils = dict() self._obj_msg_store=MessageStoreProxy(collection="soma") self._roi_msg_store=MessageStoreProxy(collection="soma_roi") if config_file: self._config_file = config_file else: # default file rp = RosPack() path = rp.get_path('soma_utils') + '/config/' filename = 'maps.json' self._config_file=path+filename self._init_maps()
def getNodes(): #print queries.get_nodes(""," ") msg_store = MessageStoreProxy(collection="topological_maps") objs = msg_store.query(TopologicalNode._type, {"pointset": "dynamic_tmap"}) nodes, metas = zip(*objs) return nodes
def add_localise_by_topic(tmap, node, json_str): #print req #data = json.loads(req.content) #print data msg_store = MessageStoreProxy(collection='topological_maps') query = {"name": node, "pointset": tmap} query_meta = {} #query_meta["pointset"] = tmap #query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query( topological_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) if len(available) != 1: #succeded = False print 'there are no nodes or more than 1 with that name' else: #succeded = True for i in available: if not i[0].localise_by_topic: msgid = i[1]['_id'] i[0].localise_by_topic = json_str #print i[0] print "Updating %s--%s" % (i[0].pointset, i[0].name) msg_store.update_id(msgid, i[0], i[1], upsert=False)
def gen_blog_entry(self, roi_id, uuid): print 'Region: ' + self.get_roi_name(roi_id) time = dt.fromtimestamp(int(rospy.get_time())) body = '### INTRUSION DETECTION REPORT\n\n' body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n' body += '- **Person UUID:** ' + str(uuid) + '\n\n' body += '- **Time:** ' + str(time) + '\n\n' #body += '- **Summary**: <font color="green">ALLOWED ITEMS (' + str(len(pos_objs)) + ')</font>, <font color="red">NOT-ALLOWED ITEMS (' + str(len(neg_objs)) + ')</font>\n\n' # # Create some blog entries msg_store = MessageStoreProxy(collection=self.blog_collection) robblog_path = roslib.packages.get_pkg_dir('soma_utils') img = rospy.wait_for_message('/upper_body_detector/image', Image, 5) bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="bgr8") ros_img = bridge.cv2_to_imgmsg(cv_image) img_id = msg_store.insert(ros_img) body += '<font color="red">Detected person:</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id e = RobblogEntry(title=str(time) + " Intrusion Detection - " + self.get_roi_name(roi_id), body= body ) msg_store.insert(e)
def get_maps(): """ Queries the database and returns details of the available topological maps. :return: A dictionary where each key is the name of a topological map and each item is a dictionary of details. Details are: "number_nodes" ; integer "edge_actions" : list of action server names used for traversal "last_modified" : datetime.datetime object for the last time a node was inserted """ maps = dict() msg_store = MessageStoreProxy(collection='topological_maps') nodes = msg_store.query(TopologicalNode._type) for node in nodes: pointset = node[1]["pointset"] if not maps.has_key(pointset): maps[pointset] = { "number_nodes": 0, "edge_actions": set(), "last_modified": "" } maps[pointset]["number_nodes"] += 1 if (maps[pointset]["last_modified"] == "" or node[1]["inserted_at"] > maps[pointset]["last_modified"]): maps[pointset]["last_modified"] = node[1]["inserted_at"] for edge in node[0].edges: maps[pointset]["edge_actions"].add(edge.action) return maps
def __init__(self): self.db_name = rospy.get_param('robot/database', 'jsk_robot_lifelog') try: self.col_name = rospy.get_param('robot/name') except KeyError as e: rospy.logerr( "please specify param \"robot/name\" (e.g. pr1012, olive)") exit(1) self.update_cycle = rospy.get_param('~update_cycle', 1.0) self.map_frame = rospy.get_param('~map_frame', 'map') self.robot_frame = rospy.get_param('~robot_frame', 'base_link') self.tf_listener = tf2_ros.BufferClient("tf2_buffer_server") self.initialpose_pub = rospy.Publisher( '/initialpose', geometry_msgs.msg.PoseWithCovarianceStamped) self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) rospy.loginfo("map->robot: %s -> %s" % (self.map_frame, self.robot_frame)) self.current_pose = None self.latest_pose = None self.latest_stamp = rospy.Time(0) self._load_latest_pose() self.pub_latest_pose() self.latest_exception = None
def __init__(self, obj_map, obj_conf, roi_map, roi_conf, config_file=None): self._soma_obj_roi_ids = dict() self._soma_obj_type = dict() self._soma_obj_pose = dict() self.obj_map = obj_map self.obj_conf = obj_conf self.roi_map = roi_map self.roi_conf = roi_conf self._soma_roi = dict() self._obj_msg_store = MessageStoreProxy(collection="soma") self._roi_msg_store = MessageStoreProxy(collection="soma_roi") self._retrieve_objects() self._retrieve_rois() self._server = InteractiveMarkerServer("soma_vis") 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._init_types()
def loadSchedule(self, givenTime): rospy.loginfo("Looking for schedules in MongoDB.") msg_store = MessageStoreProxy(collection='exploration_schedules') query = {"midnight": givenTime} available = msg_store.query(ExplorationSchedule._type, query, {}) if len(available) < 1: succeded = False rospy.loginfo("No schedules were found!") else: # Iterate through available array. # Construct message using K:V pairs. succeded = True rospy.loginfo("Schedule found... loading and publishing message!") load_schedule = ExplorationSchedule() #print available[0][0].timeInfo load_schedule.timeInfo = available[0][0].timeInfo load_schedule.entropy = available[0][0].entropy load_schedule.nodeID = available[0][0].nodeID load_schedule.midnight = available[0][0].midnight load_schedule.mode = available[0][0].mode self.soma_schedule = load_schedule return succeded
def add_edge(self, or_waypoint, de_waypoint, action) : #print 'removing edge: '+edge_name rospy.loginfo('Adding Edge from '+or_waypoint+' to '+de_waypoint+' using '+action) node_name = or_waypoint #nodeindx = self._get_node_index(edged[0]) msg_store = MessageStoreProxy(collection='topological_maps') query = {"name" : node_name, "pointset": self.name} query_meta = {} query_meta["pointset"] = self.name query_meta["map"] = self.map available = msg_store.query(TopologicalNode._type, query, query_meta) if len(available) == 1 : found =False for i in available[0][0].edges : #print i.node if i.node == de_waypoint : found=True break if not found : edge = Edge() edge.node = de_waypoint edge.action = action edge.top_vel = 0.55 available[0][0].edges.append(edge) msg_store.update(available[0][0], query_meta, query, upsert=True) else : rospy.logerr("Edge already exist: Try deleting it first") else : rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") rospy.logerr("Available data: "+str(available))
def __init__(self): self.active = True self.activate = rospy.Service('info_terminal/activate_approacher', Activate, self.activate_cb) self.card_seer = rospy.Subscriber("/socialCardReader/commands", String, self.card_callback) self.card_pose_getter = rospy.Subscriber( "/socialCardReader/cardposition", PoseStamped, self.card_pose_callback) self.tf_listener = tf.TransformListener() self.card_pose = None self.exec_status = ExecutionStatus() self.exec_status_listener = rospy.Subscriber("/current_schedule", ExecutionStatus, self.current_schedule_cb) demand_task_srv_name = '/task_executor/demand_task' set_exe_stat_srv_name = '/task_executor/set_execution_status' rospy.wait_for_service(demand_task_srv_name) rospy.wait_for_service(set_exe_stat_srv_name) self.demand_task_srv = rospy.ServiceProxy(demand_task_srv_name, DemandTask) self.set_execution_status = rospy.ServiceProxy(set_exe_stat_srv_name, SetExecutionStatus) self.msg_store_tmp = MessageStoreProxy() self.msg_store = MessageStoreProxy(collection='info_terminal_requests') self.event_pub = rospy.Publisher("/info_terminal/requests", InfoTerminalRequest, queue_size=1) print "STARTING2"
def add_tag_to_mongo(self, msg): succeded = True meta_out = None for j in msg.node: msg_store = MessageStoreProxy(collection='topological_maps') query = {"name": j, "pointset": self.nodes.name} query_meta = {} query_meta["pointset"] = self.nodes.name query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query( strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) for i in available: msgid = i[1]['_id'] if 'tag' in i[1]: if not msg.tag in i[1]['tag']: i[1]['tag'].append(msg.tag) else: a = [] a.append(msg.tag) i[1]['tag'] = a meta_out = str(i[1]) msg_store.update_id(msgid, i[0], i[1], upsert=False) #print trstr if len(available) == 0: succeded = False return succeded, meta_out
def tweet_callback(self, msg): self.msg_store = MessageStoreProxy(collection='twitter_log') to_save = strands_tweets.msg.Tweeted() try: to_save.depth = rospy.wait_for_message( '/head_xtion/depth/image_rect_meters', sensor_msgs.msg.Image, timeout=1.0) except rospy.ROSException: rospy.logwarn("Failed to get camera depth Image") try: to_save.photo = rospy.wait_for_message( '/head_xtion/rgb/image_color', sensor_msgs.msg.Image, timeout=1.0) except rospy.ROSException: rospy.logwarn("Failed to get camera rgb Image") to_save.text = msg.text meta = {} meta["Description"] = "copy of tweeted images" self.msg_store.insert(msg, meta)
def __init__(self): rospy.init_node('initial_surface_view_evaluation_actionserver', anonymous = False) rospy.loginfo("VIEW EVAL: waiting for services") self.conv_octomap = rospy.ServiceProxy('/surface_based_object_learning/convert_pcd_to_octomap',ConvertCloudToOctomap) self.get_normals = rospy.ServiceProxy('/surface_based_object_learning/extract_normals_from_octomap',ExtractNormalsFromOctomap) self.get_obs = rospy.ServiceProxy('/semantic_map_publisher/SemanticMapPublisher/ObservationService',ObservationService) self.roi_srv = rospy.ServiceProxy('/check_point_set_in_soma_roi',PointSetInROI) self.closest_roi_srv = rospy.ServiceProxy('/get_closest_roi_to_robot',GetROIClosestToRobot) #self.overlap_srv = rospy.ServiceProxy('/surface_based_object_learning/calculate_octree_overlap',CalculateOctreeOverlap) rospy.loginfo("VIEW EVAL: done") self.pc_topic = "/head_xtion/depth_registered/points" #self.pc_topic = "/head" self.rgb_topic = "/head_xtion/rgb/image_color" self.ptu_gazer_controller = PTUGazeController() self.marker_publisher = rospy.Publisher("/initial_surface_view_evaluation/centroid", Marker,queue_size=5) self.min_z_cutoff = 0.7 self.max_z_cutoff = 1.7 self.obs_resolution = 0.03 self.initial_view_store = MessageStoreProxy(database="initial_surface_views", collection="logged_views") self.segmentation = SegmentationWrapper() self.transformation_store = TransformListener() rospy.sleep(2) self.action_server = actionlib.SimpleActionServer("/surface_based_object_learning/evaluate_surface", EvaluateSurfaceAction, execute_cb=self.do_task_cb, auto_start = False) self.action_server.start() rospy.loginfo("VIEW EVAL: action server set up") rospy.spin()
def __init__(self): Thread.__init__(self) self.dead = Event() self.replicate_interval = rospy.get_param("replication/interval", 60 * 60 * 24) # default: 1 day self.delete_after_move = rospy.get_param("replication/delete_after_move", False) self.database = rospy.get_param("robot/database") self.collections = rospy.myargv()[1:] try: self.collections.append(rospy.get_param("robot/name")) except KeyError as e: rospy.logerr("specify param \"robot/name\" (e.g. pr1012, olive)") exit(1) self.periodic = rospy.get_param("replication/periodic", True) if self.periodic: rospy.loginfo("periodic replication interval: %d [sec]", self.replicate_interval) self.disable_on_wireless_network = rospy.get_param("replication/disable_on_wireless_network", False) if self.disable_on_wireless_network: self.network_connected = False self.net_sub = rospy.Subscriber("/network/connected", Bool, self.network_connected_cb) else: self.replicate_interval = 1 self.date_msg_store = MessageStoreProxy(database=self.database, collection="replication") self.replicate_ac = actionlib.SimpleActionClient('move_mongodb_entries', MoveEntriesAction) rospy.loginfo("waiting for service advertise /move_mongodb_entries ...") self.replicate_ac.wait_for_server() rospy.loginfo("replication enabled: db: %s, collection: %s, periodic: %s", self.database, self.collections, self.periodic)
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._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma") self._server = InteractiveMarkerServer("soma") self._init_types() self._init_menu() self.load_objects() rospy.spin()
def rm_tag_cb(self, msg): #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) succeded = True for j in msg.node: msg_store = MessageStoreProxy(collection='topological_maps') query = {"name" : j, "pointset": self.nodes.name} query_meta = {} query_meta["pointset"] = self.nodes.name query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) succeded = False for i in available: msgid= i[1]['_id'] if 'tag' in i[1]: if msg.tag in i[1]['tag']: print 'removing tag' i[1]['tag'].remove(msg.tag) print 'new list of tags' print i[1]['tag'] msg_store.update_id(msgid, i[0], i[1], upsert = False) succeded = True meta_out = str(i[1]) return succeded, meta_out
class logTweets(object): def __init__(self, name): rospy.loginfo(" ...starting") self.msg_sub = rospy.Subscriber('/strands_tweets/tweet', strands_tweets.msg.Tweeted, self.tweet_callback, queue_size=1) rospy.loginfo(" ...done") rospy.spin() def tweet_callback(self, msg) : self.msg_store = MessageStoreProxy(collection='twitter_log') to_save = strands_tweets.msg.Tweeted() try: to_save.depth = rospy.wait_for_message('/head_xtion/depth/image_rect_meters', sensor_msgs.msg.Image, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get camera depth Image") try: to_save.photo = rospy.wait_for_message('/head_xtion/rgb/image_color', sensor_msgs.msg.Image, timeout=1.0) except rospy.ROSException : rospy.logwarn("Failed to get camera rgb Image") to_save.text = msg.text meta = {} meta["Description"] = "copy of tweeted images" self.msg_store.insert(msg, meta)
def publish_stats(self): pubst = NavStatistics() pubst.edge_id = self.stat.edge_id pubst.status = self.stat.status pubst.origin = self.stat.origin pubst.target = self.stat.target pubst.topological_map = self.stat.topological_map pubst.final_node = self.stat.final_node pubst.time_to_waypoint = self.stat.time_to_wp pubst.operation_time = self.stat.operation_time pubst.date_started = self.stat.get_start_time_str() pubst.date_at_node = self.stat.date_at_node.strftime( '%A, %B %d %Y, at %H:%M:%S hours') pubst.date_finished = self.stat.get_finish_time_str() self.stats_pub.publish(pubst) meta = {} meta["type"] = "Topological Navigation Stat" meta["epoch"] = calendar.timegm(self.stat.date_at_node.timetuple()) meta["date"] = self.stat.date_at_node.strftime( '%A, %B %d %Y, at %H:%M:%S hours') meta["pointset"] = self.stat.topological_map msg_store = MessageStoreProxy(collection='nav_stats') msg_store.insert(pubst, meta)
def get_node_tags_from_mongo(self, req): #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) succeded = True msg_store = MessageStoreProxy(collection='topological_maps') query = {"name": req.node_name, "pointset": self.nodes.name} query_meta = {} query_meta["pointset"] = self.nodes.name query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query( strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) if len(available) == 1: # The meta information for a node is in the second part of the tuple # returned by the message store query if 'tag' in available[0][1]: tags = available[0][1]['tag'] else: tags = [] else: succeded = False tags = [] return succeded, tags
def gen_blog_entry(self, roi_id, obj_id, objs): print 'Region: ' + self.get_roi_name(roi_id) time = dt.fromtimestamp(int(rospy.get_time())) body = '### CHANGE DETECTION REPORT\n\n' body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n' #body += '- **Object ID:** ' + str(obj_id) + '\n\n' body += '- **Time:** ' + str(time) + '\n\n' # Create some blog entries msg_store = MessageStoreProxy(collection=self.blog_collection) robblog_path = roslib.packages.get_pkg_dir('soma_utils') for idx,obj in enumerate(objs): bridge = CvBridge() im = bridge.imgmsg_to_cv2(obj.image_mask, desired_encoding="bgr8") imgray = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY) ret,thresh = cv2.threshold(imgray,1,1,1) #cv2.threshold(imgray,127,255,0) contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(obj.image_full, desired_encoding="bgr8") cv2.drawContours(cv_image,contours,-1,(0,0,255),2) full_scene_contour = bridge.cv2_to_imgmsg(cv_image) img_id = msg_store.insert(full_scene_contour) body += '<font color="red">Detected change (' + str(idx+1) + '/'+ str(len(objs)) + '):</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id e = RobblogEntry(title=str(time) + " Change Detection - " + self.roi_name[roi_id], body= body ) msg_store.insert(e)
def add_edge(self, or_waypoint, de_waypoint, action): #print 'removing edge: '+edge_name rospy.loginfo('Adding Edge from ' + or_waypoint + ' to ' + de_waypoint + ' using ' + action) node_name = or_waypoint #nodeindx = self._get_node_index(edged[0]) msg_store = MessageStoreProxy(collection='topological_maps') query = {"name": node_name, "pointset": self.name} query_meta = {} query_meta["pointset"] = self.name query_meta["map"] = self.map available = msg_store.query(TopologicalNode._type, query, query_meta) if len(available) == 1: found = False for i in available[0][0].edges: #print i.node if i.node == de_waypoint: found = True break if not found: edge = Edge() edge.node = de_waypoint edge.action = action edge.top_vel = 0.55 available[0][0].edges.append(edge) msg_store.update(available[0][0], query_meta, query, upsert=True) else: rospy.logerr("Edge already exist: Try deleting it first") else: rospy.logerr("Impossible to store in DB " + str(len(available)) + " waypoints found after query") rospy.logerr("Available data: " + str(available))
def __init__(self, name, soma_map, soma_config, minute_interval, window_interval, collection="occurrence_rates"): self.topo_map = None self.soma_map = soma_map self.soma_config = soma_config self.minute_interval = minute_interval self.window_interval = window_interval self.rotm = RegionObservationTimeManager(soma_map, soma_config) self.tre = TrajectoryRegionEstimate(soma_map, soma_config, minute_interval) self.tof = TrajectoryOccurrenceFrequencies( soma_map, soma_config, minute_interval=minute_interval, window_interval=window_interval) self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma') rospy.loginfo("Connect to database collection %s..." % collection) self._ms = MessageStoreProxy(collection=collection) self._soma_db = MessageStoreProxy(collection="soma_roi") rospy.loginfo("Create a service %s/service..." % name) self.service = rospy.Service(name + '/service', TrajectoryOccurrenceRate, self.srv_cb) rospy.loginfo("Create an action server %s..." % name) self._as = actionlib.SimpleActionServer(name, OccurrenceRateLearningAction, execute_cb=self.execute, auto_start=False) self._as.start()
def add_topological_node(self, node_name, node_pose, add_close_nodes, dist=8.0): #Get New Node Name if node_name: name = node_name else: name = self.get_new_name() rospy.loginfo('Creating Node: '+name) if name in self.names: rospy.logerr("Node already exists, try another name") return False #Create Message store msg_store = MessageStoreProxy(collection='topological_maps') meta = {} meta["map"] = self.nodes.map meta["pointset"] = self.nodes.name meta["node"] = name node = strands_navigation_msgs.msg.TopologicalNode() node.name = name node.map = self.nodes.map node.pointset = self.name node.pose = node_pose node.yaw_goal_tolerance = self.yaw_goal_tolerance node.xy_goal_tolerance = self.xy_goal_tolerance node.localise_by_topic = '' vertices=self.generate_circle_vertices() for j in vertices : v = strands_navigation_msgs.msg.Vertex() v.x = float(j[0]) v.y = float(j[1]) node.verts.append(v) if add_close_nodes: close_nodes = [] for i in self.nodes.nodes: ndist = node_dist(node, i) if ndist < dist : if i.name != 'ChargingPoint': close_nodes.append(i.name) for i in close_nodes: e = strands_navigation_msgs.msg.Edge() e.node = i e.action = 'move_base' eid = '%s_%s' %(node.name, i) e.edge_id = eid e.top_vel =0.55 e.map_2d = node.map node.edges.append(e) for i in close_nodes: self.add_edge(i, node.name, 'move_base', '') msg_store.insert(node,meta) return True
def create_master_task(): """ Create an example of a task which we'll copy for other tasks later. This is a good example of creating a task with a variety of arguments. """ # need message store to pass objects around msg_store = MessageStoreProxy() # get the pose of a named object pose_name = "my favourite pose" # get the pose if it's there message, meta = msg_store.query_named(pose_name, Pose._type) # if it's not there, add it in if message == None: message = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) pose_id = msg_store.insert_named(pose_name, message) else: pose_id = meta["_id"] master_task = Task(action='test_task') task_utils.add_string_argument(master_task, 'hello world') task_utils.add_object_id_argument(master_task, pose_id, Pose) task_utils.add_int_argument(master_task, 24) task_utils.add_float_argument(master_task, 63.678) return master_task
def __init__(self): self.add_task = rospy.ServiceProxy('/robot_routine/add_tasks', AddTasks) self.task_event_sub = rospy.Subscriber('/task_executor/events', TaskEvent, self.process_task_event, queue_size = None) self.get_info_srv = rospy.Service('~get_bid_info', Empty, self.get_info_cb) #REAL self.period = rospy.Duration(rospy.get_param('~period', 60*60*24)) self.tokens_per_period = rospy.get_param('/exploration_bidding/tokens_per_period', float(60*60*12)/3) ##TESTING #self.period = rospy.Duration(rospy.get_param('~period', 60)) #self.tokens_per_period = rospy.get_param('/exploration_bidding/tokens_per_period', 20) self.available_tokens = self.tokens_per_period self.currently_bid_tokens = 0 #total number of tokens that this node has currently in play self.currently_added_tokens = 0 #total number of tokens that this node has added to the executor (can never be more than the available tokens) self.added_tasks = {} self.queued_tasks = [] self.mongo = MessageStoreProxy(collection='exploration_tasks') self.timer=rospy.Timer(self.period, self.update_budget)
def __init__(self): self.skeleton_data = {} #keeps the published complete skeleton in a dictionary. key = uuid self.rate = rospy.Rate(15.0) ## listeners: rospy.Subscriber("skeleton_data/complete", skeleton_complete, self.skeleton_callback) ## Logging params: self._with_mongodb = rospy.get_param("~with_mongodb", "true") self._logging = rospy.get_param("~logging", "false") self._message_store = rospy.get_param("~message_store_prefix", "people_skeleton") if self._logging: msg_store = self._message_store + "_world_state" rospy.loginfo("Connecting to mongodb...%s" % msg_store) self._store_client_world = MessageStoreProxy(collection=msg_store) msg_store = self._message_store + "_qstag" self._store_client_qstag = MessageStoreProxy(collection=msg_store) ## QSR INFO: self._which_qsr = rospy.get_param("~which_qsr", "rcc3") #self.which_qsr = ["qtcbs", "argd"] self.set_params() self.cln = QSRlib_ROS_Client()
def loadMap(self, point_set, filename): point_set = str(sys.argv[1]) #map_name=str(sys.argv[3]) msg_store = MessageStoreProxy(collection='topological_maps') query_meta = {} query_meta["pointset"] = point_set available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 print available if available <= 0: rospy.logerr("Desired pointset '" + point_set + "' not in datacentre") rospy.logerr("Available pointsets: " + str(available)) raise Exception("Can't find waypoints.") else: query_meta = {} query_meta["pointset"] = point_set message_list = msg_store.query(TopologicalNode._type, {}, query_meta) #node=TopologicalNode() top_map = [] for i in message_list: nodeinf = {} nodeinf["node"] = yaml.load(str(i[0])) if nodeinf["node"]["localise_by_topic"]: nodeinf["node"]["localise_by_topic"] = json.dumps( nodeinf["node"]["localise_by_topic"]) nodeinf["meta"] = i[ 1] #str(bson.json_util.dumps(i[1], indent=1)) nodeinf["meta"].pop("last_updated_by", None) nodeinf["meta"].pop('inserted_at', None) nodeinf["meta"].pop('last_updated_at', None) nodeinf["meta"].pop('stored_type', None) nodeinf["meta"].pop('stored_class', None) nodeinf["meta"].pop('inserted_by', None) nodeinf["meta"].pop('_id', None) top_map.append(nodeinf) #val = bson.json_util.dumps(nodeinf["meta"], indent=1) top_map.sort(key=lambda x: x['node']['name']) yml = yaml.safe_dump(top_map, default_flow_style=False) #print yml #print s_output fh = open(filename, "w") #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) s_output = str(yml) #print s_output fh.write(s_output) fh.close
def get_all_blocks_from_db(): msg_store = MessageStoreProxy() res = msg_store.query(Block._type) (blocks, metas) = zip(*res) return blocks
def rm_tag_cb(self, msg): #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) succeded = True for j in msg.node: msg_store = MessageStoreProxy(collection='topological_maps') query = {"name": j, "pointset": self.nodes.name} query_meta = {} query_meta["pointset"] = self.nodes.name query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query( strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) succeded = False meta_out = None for i in available: msgid = i[1]['_id'] if 'tag' in i[1]: if msg.tag in i[1]['tag']: print 'removing tag' i[1]['tag'].remove(msg.tag) print 'new list of tags' print i[1]['tag'] msg_store.update_id(msgid, i[0], i[1], upsert=False) succeded = True meta_out = str(i[1]) return succeded, meta_out
def publish_topological_map(): """ Publish a topological map for testing. """ # create a test topological map width = 5 height = 5 nodeSeparation = 10.0 test_nodes = topological_navigation.testing.create_cross_map( width=width, height=height, nodeSeparation=nodeSeparation) # now insert the map into the database msg_store = MessageStoreProxy(collection='topological_maps') map_name = 'test_top_map' meta = {} meta['map'] = map_name meta['pointset'] = map_name for (nodeName, node) in test_nodes.iteritems(): meta["node"] = nodeName node.map = meta['map'] node.pointset = meta['pointset'] msg_store.insert(node, meta) # and publish the map ps = map_publisher(map_name) return test_nodes
def modify_tag_cb(self, msg): succeded = True meta_out = None for node in msg.node: msg_store = MessageStoreProxy(collection='topological_maps') query = {"name": node, "pointset": self.nodes.name} query_meta = {} query_meta["pointset"] = self.nodes.name query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query( strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) for node_plus_meta in available: msgid = node_plus_meta[1]['_id'] if 'tag' in node_plus_meta[1]: if not msg.tag in node_plus_meta[1]['tag']: continue else: tag_ind = node_plus_meta[1]['tag'].index(msg.tag) node_plus_meta[1]['tag'][tag_ind] = msg.new_tag meta_out = str(node_plus_meta[1]) msg_store.update_id(msgid, node_plus_meta[0], node_plus_meta[1], upsert=True) if len(available) == 0: succeded = False return succeded, meta_out
def __init__(self, obj_map, obj_conf, roi_map, roi_conf, config_file=None): self._soma_obj_roi_ids = dict() self._soma_obj_type = dict() self._soma_obj_pose = dict() self.obj_map = obj_map self.obj_conf = obj_conf self.roi_map = roi_map self.roi_conf = roi_conf self._soma_roi = dict() self._obj_msg_store=MessageStoreProxy(collection="soma") self._roi_msg_store=MessageStoreProxy(collection="soma_roi") self._retrieve_objects() self._retrieve_rois() self._server = InteractiveMarkerServer("soma_vis") 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._init_types()
def __init__(self): self.skeleton_data = { } #keeps the published complete skeleton in a dictionary. key = uuid self.rate = rospy.Rate(15.0) ## listeners: rospy.Subscriber("skeleton_data/complete", skeleton_complete, self.skeleton_callback) ## Logging params: self._with_mongodb = rospy.get_param("~with_mongodb", "true") self._logging = rospy.get_param("~logging", "false") self._message_store = rospy.get_param("~message_store_prefix", "people_skeleton") if self._logging: msg_store = self._message_store + "_world_state" rospy.loginfo("Connecting to mongodb...%s" % msg_store) self._store_client_world = MessageStoreProxy(collection=msg_store) msg_store = self._message_store + "_qstag" self._store_client_qstag = MessageStoreProxy(collection=msg_store) ## QSR INFO: self._which_qsr = rospy.get_param("~which_qsr", "rcc3") #self.which_qsr = ["qtcbs", "argd"] self.set_params() self.cln = QSRlib_ROS_Client()
def __init__(self, path, recordings): print "initialise activity learning action class" self.path = path self.recordings = recordings self.load_config( ) # loads all the learning parameters from a config file self.path = '/home/' + getpass.getuser() + '/SkeletonDataset/' self.make_init_filepaths() # Define all file paths self.recordings = recordings # self.soma_map = rospy.get_param("~soma_map", "collect_data_map_cleaned") # self.soma_config = rospy.get_param("~soma_config", "test") # objects # self.roi_config = rospy.get_param("~roi_config", "test") # regions self.soma_map = self.config['events']['soma_map'] self.soma_config = self.config['events']['soma_config'] self.roi_config = self.config['events']['roi_config'] # SOMA services rospy.loginfo("Wait for soma roi service") rospy.wait_for_service('/soma/query_rois') self.soma_query = rospy.ServiceProxy('/soma/query_rois', SOMAQueryROIs) rospy.loginfo("Done") self.soma_id_store = MessageStoreProxy( database='message_store', collection='soma_activity_ids_list') self.soma_store = MessageStoreProxy(database="somadata", collection="object") self.olda_msg_store = MessageStoreProxy( database='message_store', collection='activity_topic_models')
def add_localise_by_topic(tmap, node, json_str): #print req #data = json.loads(req.content) #print data msg_store = MessageStoreProxy(collection='topological_maps') query = {"name" : node, "pointset": tmap} query_meta = {} #query_meta["pointset"] = tmap #query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) if len(available) != 1: #succeded = False print 'there are no nodes or more than 1 with that name' else: #succeded = True for i in available: if not i[0].localise_by_topic: msgid= i[1]['_id'] i[0].localise_by_topic=json_str #print i[0] print "Updating %s--%s" %(i[0].pointset, i[0].name) msg_store.update_id(msgid, i[0], i[1], upsert = False)
def add_tag_cb(self, msg): #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node)) succeded = True for j in msg.node: msg_store = MessageStoreProxy(collection='topological_maps') query = {"name" : j, "pointset": self.nodes.name} query_meta = {} query_meta["pointset"] = self.nodes.name query_meta["map"] = self.nodes.map #print query, query_meta available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta) #print len(available) for i in available: msgid= i[1]['_id'] if 'tag' in i[1]: if not msg.tag in i[1]['tag']: i[1]['tag'].append(msg.tag) else: a=[] a.append(msg.tag) i[1]['tag']=a meta_out = str(i[1]) msg_store.update_id(msgid, i[0], i[1], upsert = False) #print trstr if len(available) == 0: succeded = False return succeded, meta_out
def add_topological_node(self, node_name, node_pose): dist = 8.0 #Get New Node Name if node_name: name = node_name else: name = self.get_new_name() rospy.loginfo('Creating Node: '+name) if name in self.names: rospy.logerr("Node already exists, try another name") return False #Create Message store msg_store = MessageStoreProxy(collection='topological_maps') meta = {} meta["map"] = self.nodes.map meta["pointset"] = self.nodes.name meta["node"] = name node = strands_navigation_msgs.msg.TopologicalNode() node.name = name node.map = self.nodes.map node.pointset = self.name node.pose = node_pose node.yaw_goal_tolerance = 0.1 node.xy_goal_tolerance = 0.3 node.localise_by_topic = '' vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)] for j in vertices : v = strands_navigation_msgs.msg.Vertex() v.x = float(j[0]) v.y = float(j[1]) node.verts.append(v) close_nodes = [] for i in self.nodes.nodes : ndist = node_dist(node, i) if ndist < dist : if i.name != 'ChargingPoint' : close_nodes.append(i.name) for i in close_nodes: e = strands_navigation_msgs.msg.Edge() e.node = i e.action = 'move_base' eid = '%s_%s' %(node.name, i) e.edge_id = eid e.top_vel =0.55 e.map_2d = node.map node.edges.append(e) for i in close_nodes: self.add_edge(i, node.name, 'move_base', '') msg_store.insert(node,meta) return True
def get_soma_objects(self): """srv call to mongo and get the list of new objects and locations""" msg_store = MessageStoreProxy(database="soma2data", collection="soma2") objs = msg_store.query(SOMA2Object._type, message_query={"map_name":self.soma_map,"config":self.soma_conf}) print "queried soma2 objects >> ", objs self.soma_objects = ce.get_soma_objects() print "hard coded objects >> ", [self.soma_objects[r].keys() for r in self.soma_objects.keys()]
class PeriodicReplicatorClient(Thread): def __init__(self): Thread.__init__(self) self.dead = Event() self.interval = rospy.get_param("mongodb_replication_interval", 60 * 60 * 24) # default: 1 day self.delete_after_move = rospy.get_param("mongodb_replication_delete_after_move", False) self.replicate_interval = rospy.Duration(self.interval) self.database = rospy.get_param("robot/database") self.collections = sys.argv[2:] try: self.collections.append(rospy.get_param("robot/name")) except KeyError as e: rospy.logerr("specify param \"robot/name\" (e.g. pr1012, olive)") exit(1) self.periodic = rospy.get_param("~periodic", True) self.date_msg_store = MessageStoreProxy(database=self.database, collection="replication") def run(self): while not self.dead.wait(self.interval): move_before = self.time_after_last_replicate_date() self.move_entries(move_before) self.insert_replicate_date() def time_after_last_replicate_date(self): delta = 60 * 60 * 24 # 1 day try: last_replicated = self.date_msg_store.query(StringList._type, single=True, sort_query=[("$natural",-1)]) date = last_replicated[1]["inserted_at"] rospy.loginfo("last replicated at %s", date) delta = (dt.datetime.now() - date).seconds + 60 except Exception as e: rospy.logwarn("failed to search last replicated date from database: %s", e) finally: return rospy.Duration(delta) def insert_replicate_date(self): try: self.date_msg_store.insert(StringList(self.collections)) except Exception as e: rospy.logwarn("failed to insert last replicate date to database: %s", e) def move_entries(self, move_before): client = actionlib.SimpleActionClient('move_mongodb_entries', MoveEntriesAction) client.wait_for_server() goal = MoveEntriesGoal(database=self.database, collections=StringList(self.collections), move_before=move_before, delete_after_move=self.delete_after_move) client.send_goal(goal, feedback_cb=self.feedback_cb) client.wait_for_result() def feedback_cb(self, feedback): rospy.loginfo(feedback) def cancel(self): self.dead.set()
def loadMap(self, point_set, filename): point_set=str(sys.argv[1]) #map_name=str(sys.argv[3]) msg_store = MessageStoreProxy(collection='topological_maps') query_meta = {} query_meta["pointset"] = point_set available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 print available if available <= 0 : rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") rospy.logerr("Available pointsets: "+str(available)) raise Exception("Can't find waypoints.") else : query_meta = {} query_meta["pointset"] = point_set message_list = msg_store.query(TopologicalNode._type, {}, query_meta) #node=TopologicalNode() top_map=[] for i in message_list: nodeinf = {} nodeinf["node"] = yaml.load(str(i[0])) if nodeinf["node"]["localise_by_topic"]: nodeinf["node"]["localise_by_topic"] = json.dumps(nodeinf["node"]["localise_by_topic"]) nodeinf["meta"] = i[1] #str(bson.json_util.dumps(i[1], indent=1)) nodeinf["meta"].pop("last_updated_by", None) nodeinf["meta"].pop('inserted_at', None) nodeinf["meta"].pop('last_updated_at', None) nodeinf["meta"].pop('stored_type', None) nodeinf["meta"].pop('stored_class', None) nodeinf["meta"].pop('inserted_by', None) nodeinf["meta"].pop('_id', None) top_map.append(nodeinf) #val = bson.json_util.dumps(nodeinf["meta"], indent=1) top_map.sort(key=lambda x: x['node']['name']) yml = yaml.safe_dump(top_map, default_flow_style=False) #print yml #print s_output fh = open(filename, "w") #s_output = str(bson.json_util.dumps(nodeinf, indent=1)) s_output = str(yml) #print s_output fh.write(s_output) fh.close
def get_message_store_messages(self, typ=None): msgs = [] proxy = MessageStoreProxy() for msg in self._msg_store_objects: if typ != msg.typ and typ is not None: continue proxy.database = msg.database proxy.collection = msg.collection msgs.append(proxy.query_id(msg.obj_id, msg.typ)[0]) return msgs
def loadConfig(self, data_set): msg_store = MessageStoreProxy(collection="hri_behaviours") query_meta = {} query_meta["collection"] = data_set if len(msg_store.query(std_msgs.msg.String._type, {}, query_meta)) == 0: rospy.logerr("Desired dialogue options '"+data_set+"' not in datacentre.") raise Exception("Can't find data in datacentre.") else: message = msg_store.query(std_msgs.msg.String._type, {}, query_meta) return json.loads(message[0][0].data)
def rename_node(name, new_name, top_map_name): """ Renames a topological map node. All edges will be updated to reflect the change. @param name: the current name of the node to be changec @param new_name: the new name to give the node @param top_map_name: the name of the topological map to work with (pointset) @return (number of nodes changed, number of edges changed) """ maps = queries.get_maps() if top_map_name not in maps: raise Exception("Unknown topological map.") msg_store = MessageStoreProxy(collection='topological_maps') nodes = msg_store.query(TopologicalNode._type, {}, {'pointset':top_map_name}) node_names = [node.name for node,meta in nodes] node_renames = 0 edge_changes = 0 node_changes = 0 if name not in node_names: raise Exception("No such node.") if new_name in node_names: raise Exception("New node name already in use.") old_metas = [] for node, meta in nodes: old_metas.append(copy.deepcopy(meta)) if meta["node"] == name: meta["node"] = new_name if node.name == name: node.name = new_name node_renames += 1 if node_renames > 1: raise Exception("More than one node has the same name!") for edge in node.edges: if edge.node == name: edge.node = new_name if edge.edge_id.find(name) > -1: edge.edge_id = edge.edge_id.replace(name, new_name) edge_changes += 1 # Save the changed nodes for ((node, meta), old_meta) in zip(nodes, old_metas): changed = msg_store.update(node, meta, {'name':old_meta['node'], 'pointset':meta['pointset']}) if changed.success: node_changes += 1 return (node_changes, edge_changes)
def delete_map(self) : rospy.loginfo('Deleting map: '+self.name) msg_store = MessageStoreProxy(collection='topological_maps') query_meta = {} query_meta["pointset"] = self.name message_list = msg_store.query(TopologicalNode._type, {}, query_meta) for i in message_list: rm_id = str(i[1]['_id']) msg_store.delete(rm_id)
def add_node(self, name, dist, pos, std_action) : rospy.loginfo('Creating Node: '+name) msg_store = MessageStoreProxy(collection='topological_maps') found = False for i in self.nodes : if i.name == name : found = True if found : rospy.logerr("Node already exists, try another name") else : rospy.loginfo('Adding node: '+name) meta = {} meta["map"] = self.map meta["pointset"] = self.name meta["node"] = name node = TopologicalNode() node.name = name node.map = self.map node.pointset = self.name node.pose = pos vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)] for j in vertices : v = Vertex() v.x = float(j[0]) v.y = float(j[1]) node.verts.append(v) cx = node.pose.position.x cy = node.pose.position.y close_nodes = [] for i in self.nodes : ndist = i._get_distance(cx, cy) if ndist < dist : if i.name != 'ChargingPoint' : close_nodes.append(i.name) for i in close_nodes : edge = Edge() edge.node = i edge.action = std_action node.edges.append(edge) msg_store.insert(node,meta) for i in close_nodes : self.add_edge(i, name, std_action)
def loadMap(self, point_set): point_set=str(sys.argv[1]) #map_name=str(sys.argv[3]) msg_store = MessageStoreProxy(collection='topological_maps') query_meta = {} query_meta["pointset"] = point_set available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0 print available if available <= 0 : rospy.logerr("Desired pointset '"+point_set+"' not in datacentre") rospy.logerr("Available pointsets: "+str(available)) raise Exception("Can't find waypoints.") else : query_meta = {} query_meta["pointset"] = point_set message_list = msg_store.query(TopologicalNode._type, {}, query_meta) points = [] for i in message_list: #print i[0].name b = topological_node(i[0].name) edges = [] for j in i[0].edges : data = {} data["node"]=j.node data["action"]=j.action edges.append(data) b.edges = edges verts = [] for j in i[0].verts : data = [j.x,j.y] verts.append(data) b._insert_vertices(verts) c=i[0].pose waypoint=[str(c.position.x), str(c.position.y), str(c.position.z), str(c.orientation.x), str(c.orientation.y), str(c.orientation.z), str(c.orientation.w)] b.waypoint = waypoint b._get_coords() points.append(b) return points
def gather_stats(self): stats_collection = rospy.get_param('~stats_collection', 'nav_stats') msg_store = MessageStoreProxy(collection=stats_collection) to_add=[] for i in self.eids: query = {"topological_map": self.lnodes.name, "edge_id":i["edge_id"]} query_meta={} if len(self.range) == 2: if self.range[1] < 1: upperlim = rospy.Time.now().secs else: upperlim = self.range[1] query_meta["epoch"] = {"$gte": self.range[0], "$lt" : upperlim} #print query #print query_meta available = msg_store.query(NavStatistics._type, query, query_meta) # print len(available) edge_mod={} edge_mod["model_id"]= i["model_id"]#self.lnodes.name+'__'+i["edge_id"] edge_mod["time_model_id"]=i["time_model_id"] edge_mod["dist"]= i["dist"]#self.lnodes.name+'__'+i["edge_id"] edge_mod["models"]=[] edge_mod["order"]=-1 edge_mod["t_order"]=-1 edge_mod["edge_id"]=i["edge_id"] for j in available: val = {} if j[0].status != 'fatal': val["st"] = 1 val["speed"] = i["dist"]/j[0].operation_time if val["speed"]>1: val["speed"]=1.0 else: val["st"] = 0 val["speed"] = 0.0 val["epoch"] = int(datetime.strptime(j[0].date_started, "%A, %B %d %Y, at %H:%M:%S hours").strftime('%s')) val["optime"] = j[0].operation_time edge_mod["models"].append(val) if len(available) > 0 : to_add.append(edge_mod) else : self.unknowns.append(edge_mod) return to_add
def update_node_waypoint(self, node_name, new_pose) : msg_store = MessageStoreProxy(collection='topological_maps') query = {"name" : node_name, "pointset": self.name} query_meta = {} query_meta["pointset"] = self.name query_meta["map"] = self.map available = msg_store.query(TopologicalNode._type, query, query_meta) if len(available) == 1 : positionZ=available[0][0].pose.position.z available[0][0].pose = new_pose available[0][0].pose.position.z = positionZ msg_store.update(available[0][0], query_meta, query, upsert=True) else : rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query") rospy.logerr("Available data: "+str(available))
def export_data(tmap, data_window_start, data_window_end, data_window_size, data_windows): msg_store = MessageStoreProxy(collection='nav_stats') for window in range(data_windows): window_start = data_window_start + data_window_size * window window_end = data_window_end + data_window_size * window # get nav stats for window meta_query = {"epoch" : {"$gt": to_epoch(window_start), "$lt" : to_epoch(window_end)}} nav_stats = msg_store.query(NavStatistics._type, {'topological_map': tmap}, meta_query) write_stats(window_start, window_end, nav_stats)
def create_datacentre_task(to_replicate, delete_after_move=True): task = Task() # no idea, let's say 2 hours for now -- this action server can't be preempted though, so this is cheating task.max_duration = rospy.Duration(60 * 60 * 2) task.action = 'move_mongodb_entries' # add arg for collectionst o replication collections = StringList(to_replicate) msg_store = MessageStoreProxy() object_id = msg_store.insert(collections) task_utils.add_object_id_argument(task, object_id, StringList) # move stuff over 24 hours old task_utils.add_duration_argument(task, rospy.Duration(60 * 60 *24)) # and delete afterwards task_utils.add_bool_argument(task, delete_after_move) return task
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_roi_manager') + '/config/' filename = 'default.json' self._config_file=path+filename self._soma_obj_ids = dict() self._soma_obj_msg = dict() self._soma_obj_roi_ids = dict() self._soma_obj_roi = dict() self._soma_obj_type = dict() self._soma_obj_pose = dict() self._interactive = True self._msg_store=MessageStoreProxy(collection="soma_roi") self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma") self._server = InteractiveMarkerServer("soma_roi") self._init_types() self._init_menu() self.load_objects() rospy.spin()
class TrajectoryQueryService(): def __init__(self): self.gs = GeoSpatialStoreProxy('geospatial_store','soma') self.ms = MessageStoreProxy(collection="people_trajectory") # setting up the service self.ser = rospy.Service('/trajectory_query', TrajectoryQuery, self.service_cb) self.topic = 'trajectory_query' self.vis = TrajectoryVisualizer(self.topic) def retrieve_msg(self, uuid): res = self.ms.query(Trajectory._type, message_query={"uuid": uuid}) if len(res) < 1: rospy.logerr("Trajectory not found: %s" % uuid) return None elif len(res) > 1: rospy.logerr("Multiple trajectories found: %s" % uuid) t = res[0][0] return t t = res[0][0] return t def service_cb(self, req): rospy.loginfo("Request received: %s" % req) if req.visualize: self.vis.clear() res = TrajectoryQueryResponse() res.trajectories = Trajectories() try: json_query = json.loads(req.query) trajectories = self.gs.find(json_query) except: rospy.logerr("Invalid json => re-check syntax") res.error = True return res count = 0 for t in trajectories: if t.has_key('uuid'): count += 1 #rospy.loginfo("retrieve msg for uuid: %s" % t['uuid']) # otherwise result is not a trajectory => ignore msg = self.retrieve_msg(t['uuid']) if msg: res.trajectories.trajectories.append(msg) rospy.loginfo("Query result: %s trajectories" % count) if req.visualize: rospy.loginfo("Visualize result on topic: %s" % self.topic) self.vis.visualize_trajectories(res.trajectories) rospy.loginfo("Response returned") res.error = False return res def main(self): rospy.spin()
class LoggerBase(object): def __init__(self, db_name='jsk_robot_lifelog', col_name=None): self.db_name = rospy.get_param('robot/database','jsk_robot_lifelog') try: if col_name is None: self.col_name = rospy.get_param('robot/name') else: self.col_name = col_name except KeyError as e: rospy.logerr("please specify param \"robot/name\" (e.g. pr1012, olive)") exit(1) self.update_cycle = rospy.get_param("update_cycle", 1) self.task_id = None self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name) rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name)) def insert(self, msg, meta={}, wait=False): if self.task_id is not None: meta.update({ "task_id": self.task_id }) return self.msg_store.insert(msg, meta, wait=wait) def spinOnce(self): rospy.sleep(self.update_cycle) self.task_id = rospy.get_param("/task_id", None)