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)\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)\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)\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)
