class TrajectoryImporter(object):
def __init__(self, online):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if not online:
# self._client = pymongo.MongoClient(rospy.get_param("mongodb_host"),
# rospy.get_param("mongodb_port"))
# self._store_client = MessageStoreProxy(
# collection="people_trajectory"
# )
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
else:
rospy.loginfo("Subscribing to /human_trajectories/trajectories...")
rospy.Subscriber("/human_trajectories/trajectories/complete",
Trajectories,
self.traj_callback,
None,
queue_size=10)
def store_all(self):
for uuid, t in self._traj.items():
# msg = t.get_trajectory_message()
# geo_json = self.geojson_from_trajectory(msg)
geo_json = self.geojson_from_trajectory(t)
# in case trajectory is already in there => replace
_uuid = geo_json['uuid']
self.gs.remove(_uuid)
rospy.loginfo("Storing %s data to geospatial_store", uuid)
self.gs.insert(geo_json)
# meta = dict()
# meta["map"] = map
# rospy.loginfo("Storing %s in geo
# self._store_client.insert(msg, meta)
del self._traj[uuid]
rospy.sleep(0.5)
def traj_callback(self, msg):
self._traj.update(
{traj.uuid: traj
for i, traj in enumerate(msg.trajectories)})
def geojson_from_trajectory(self, msg):
geojson = {}
# trajectory UUID
geojson['uuid'] = msg.uuid
# space
loc = {}
loc['type'] = 'LineString'
loc['coordinates'] = []
for ps in msg.trajectory:
xy_coord = self.gs.coords_to_lnglat(ps.pose.position.x,
ps.pose.position.y)
loc['coordinates'].append(xy_coord)
geojson['loc'] = loc
# time
geojson['start'] = msg.start_time.to_sec()
geojson['end'] = msg.end_time.to_sec()
start_dt = datetime.fromtimestamp(msg.start_time.to_sec())
geojson['start_weekday'] = start_dt.weekday()
geojson['start_hour'] = start_dt.hour
geojson['start_minute'] = start_dt.minute
end_dt = datetime.fromtimestamp(msg.end_time.to_sec())
geojson['end_weekday'] = end_dt.weekday()
geojson['end_hour'] = end_dt.hour
geojson['end_minute'] = end_dt.minute
return geojson
class SOMAROIManager():
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()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
for k, v in config['roi'].iteritems():
self.mesh[k] = v
def _init_menu(self):
self.menu_handler = MenuHandler()
add_point_entry = self.menu_handler.insert( "Add Point", callback=self._add_point_cb)
del_point_entry = self.menu_handler.insert( "Delete Point", callback=self._del_point_cb)
add_entry = self.menu_handler.insert( "Add ROI" )
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert( "Delete ROI", callback=self._del_cb)
enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )
def _add_cb(self, feedback):
rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete ROI: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
for r in self._soma_obj_roi_ids[roi]:
self.delete_object(r)
self.undraw_roi(roi)
def _add_point_cb(self, feedback):
rospy.loginfo("Add point: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
t = self._soma_obj_type[feedback.marker_name]
self.add_object(t, feedback.pose, roi)
#self.draw_roi(roi)
def _del_point_cb(self, feedback):
rospy.loginfo("Delete point: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
def _update_poly(self, feedback):
return
def _update_cb(self, feedback):
p = feedback.pose.position
#print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
self._soma_obj_pose[feedback.marker_name] = feedback.pose
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
if hasattr(self, "vp_timer_"+feedback.marker_name):
getattr(self, "vp_timer_"+feedback.marker_name).cancel()
setattr(self, "vp_timer_"+feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_"+feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
self._interactive = False
else:
self.menu_handler.setCheckState( handle, MenuHandler.CHECKED )
self._interactive = True
self.menu_handler.reApply( self._server )
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _next_roi_id(self):
self._soma_roi_id += 1
return self._soma_roi_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAROIObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf})
max_id = 0
max_roi_id = 0
for o,om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
if int(o.roi_id) > max_roi_id:
max_roi_id = int(o.roi_id)
self._soma_id = max_id
self._soma_roi_id = max_roi_id
return objs
def load_objects(self):
self._soma_obj_roi_ids = dict()
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Office', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
if o.roi_id in self._soma_obj_roi_ids:
self._soma_obj_roi_ids[o.roi_id].append(o.id)
else:
self._soma_obj_roi_ids[o.roi_id] = list()
self._soma_obj_roi_ids[o.roi_id].append(o.id)
self._soma_obj_roi[o.id] = o.roi_id
self._soma_obj_type[o.id] = o.type
self._soma_obj_pose[o.id] = o.pose
self.load_object(o.id, o.roi_id, o.type, o.pose)
self.draw_all_roi()
def draw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.draw_roi(key)
def undraw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.undraw_roi(key)
def draw_roi(self, roi):
v = self._soma_obj_roi_ids[roi]
t = self._soma_obj_type[v[0]]
p = self._soma_obj_pose[v[0]]
int_marker = self.create_roi_marker(roi, t, p, v)
self._server.erase("ROI-" + roi)
self._server.applyChanges()
self._server.insert(int_marker, self._update_poly)
self._server.applyChanges()
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose):
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply( self._server, soma_id )
self._server.applyChanges()
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
soma_id = self._next_id()
if roi_id == None:
soma_roi_id = self._next_roi_id()
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
else:
soma_roi_id = roi_id
soma_obj = SOMAROIObject()
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.frame = 'map'
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_roi[soma_obj.id] = soma_obj.roi_id
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_pose[soma_obj.id] = pose
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_roi_id': soma_roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(soma_obj)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (soma_obj.type, soma_obj.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (soma_type, soma_roi_id))
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_roi_id'] = soma_obj.roi_id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
if len(self._soma_obj_roi_ids[soma_obj.roi_id]) < 3:
rospy.logerr("GS Store: %s %s, less then 3 points => Add more points or delete ROI." % (soma_obj.type, soma_obj.roi_id) )
return None
coordinates = []
for obj_id in self._soma_obj_roi_ids[soma_obj.roi_id]:
p = self._soma_obj_pose[obj_id]
coordinates.append(self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
p = self._soma_obj_pose[self._soma_obj_roi_ids[soma_obj.roi_id][0]]
coordinates.append(self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
geo_json['loc'] = {'type': 'Polygon',
'coordinates': [coordinates]}
return geo_json
def delete_object(self, soma_id):
# todo: delete from mongodb
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
roi = self._soma_obj_roi[str(soma_id)]
nodes = self._soma_obj_roi_ids[roi]
new_nodes = []
for n in nodes:
if n != str(soma_id):
new_nodes.append(n)
self._soma_obj_roi_ids[roi] = new_nodes
# geospatial store
# delete old message
old_msg = self._soma_obj_msg[soma_id]
res = self._gs_store.find_one({'soma_roi_id': roi,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(old_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (old_msg.type, old_msg.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (old_msg.type, old_msg.roi_id))
def update_object(self, feedback):
rospy.loginfo("Updated marker: %s", feedback.marker_name)
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._soma_obj_pose[feedback.marker_name] = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_roi_id': new_msg.roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(new_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" % (new_msg.type, new_msg.roi_id) )
except:
rospy.logerr("The polygon of %s %s is malformed (self-intersecting) => Please update geometry." % (new_msg.type, new_msg.roi_id))
def create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control);
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append( marker )
int_marker.controls.append(control )
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
class SOMAManager():
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_objects') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
self.marker = dict()
if '2D' in config:
for k, v in config['2D'].iteritems():
self.mesh[k] = v
self.marker[k] = '2D'
if '3D' in config:
for k, v in config['3D'].iteritems():
self.mesh[k] = v
self.marker[k] = '3D'
def _init_menu(self):
self.menu_handler = MenuHandler()
add_entry = self.menu_handler.insert( "Add object" )
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k, parent=add_entry, callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert( "Delete object", callback=self._del_cb)
enable_entry = self.menu_handler.insert( "Movement control", callback=self._enable_cb )
self.menu_handler.setCheckState( enable_entry, MenuHandler.CHECKED )
def _add_cb(self, feedback):
rospy.loginfo("Add marker: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete marker: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
def _update_cb(self, feedback):
p = feedback.pose.position
print "Marker " + feedback.marker_name + " position: " + str(round(p.x,2)) + ", " + str(round(p.y,2)) + ", " + str(round(p.z,2))
if hasattr(self, "vp_timer_"+feedback.marker_name):
getattr(self, "vp_timer_"+feedback.marker_name).cancel()
setattr(self, "vp_timer_"+feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_"+feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState( handle, MenuHandler.UNCHECKED )
self._interactive = False
else:
self.menu_handler.setCheckState( handle, MenuHandler.CHECKED )
self._interactive = True
self.menu_handler.reApply( self._server )
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAObject._type, message_query={"map": self.soma_map,
"config": self.soma_conf})
max_id = 0
for o,om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
self._soma_id = max_id
return objs
def load_objects(self):
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Table', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
self.load_object(o.id, o.type, o.pose)
def load_object(self, soma_id, soma_type, pose):
int_marker = self.create_object_marker(soma_id, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply( self._server, soma_id )
self._server.applyChanges()
def add_object(self, soma_type, pose):
# todo: add to mongodb
soma_id = self._next_id()
soma_obj = SOMAObject()
soma_obj.id = str(soma_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.pose.position.z = 0.0
soma_obj.frame = 'map'
soma_obj.mesh = self.mesh[soma_type]
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
# add object to geospatial store
self._gs_store.insert(self.geo_json_from_soma_obj(soma_obj))
print "GS Store: added obj"
self.load_object(str(soma_id), soma_type, soma_obj.pose)
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_id'] = soma_obj.id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
geo_json['loc'] = {'type': 'Point',
'coordinates': self._gs_store.coords_to_lnglat(soma_obj.pose.position.x,
soma_obj.pose.position.y)}
return geo_json
def delete_object(self, soma_id):
# geospatial store
res = self._gs_store.find_one({'soma_id': soma_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
print "GS Store: deleted obj"
# message store
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
def update_object(self, feedback):
print "Updated marker " + feedback.marker_name
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({'soma_id': new_msg.id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
print "GS Store: deleted obj"
# add new object to geospatial store
self._gs_store.insert(self.geo_json_from_soma_obj(new_msg))
print "GS Store: added obj"
def create_object_marker(self, soma_obj, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = "id" + soma_obj
int_marker.pose = pose
int_marker.pose.position.z = 0.01
mesh_marker = Marker()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.scale.x = 1
mesh_marker.scale.y = 1
mesh_marker.scale.z = 1
random.seed(soma_type)
val = random.random()
mesh_marker.color.r = r_func(val)
mesh_marker.color.g = g_func(val)
mesh_marker.color.b = b_func(val)
mesh_marker.color.a = 1.0
#mesh_marker.pose = pose
mesh_marker.mesh_resource = self.mesh[soma_type]
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
if self.marker[soma_type] == '3D':
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
class TrajectoryImporter(object):
def __init__(self, online):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if not online:
# self._client = pymongo.MongoClient(rospy.get_param("mongodb_host"),
# rospy.get_param("mongodb_port"))
# self._store_client = MessageStoreProxy(
# collection="people_trajectory"
# )
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
else:
rospy.loginfo("Subscribing to /human_trajectories/trajectories...")
rospy.Subscriber(
"/human_trajectories/trajectories/complete", Trajectories,
self.traj_callback, None, queue_size=10
)
def store_all(self):
for uuid, t in self._traj.items():
# msg = t.get_trajectory_message()
# geo_json = self.geojson_from_trajectory(msg)
geo_json = self.geojson_from_trajectory(t)
# in case trajectory is already in there => replace
_uuid = geo_json['uuid']
self.gs.remove(_uuid)
rospy.loginfo("Storing %s data to geospatial_store", uuid)
self.gs.insert(geo_json)
# meta = dict()
# meta["map"] = map
# rospy.loginfo("Storing %s in geo
# self._store_client.insert(msg, meta)
del self._traj[uuid]
rospy.sleep(0.5)
def traj_callback(self, msg):
self._traj.update({
traj.uuid: traj for i, traj in enumerate(msg.trajectories)
})
def geojson_from_trajectory(self, msg):
geojson = {}
# trajectory UUID
geojson['uuid'] = msg.uuid
# space
loc = {}
loc['type'] = 'LineString'
loc['coordinates'] = []
for ps in msg.trajectory:
xy_coord = self.gs.coords_to_lnglat(
ps.pose.position.x, ps.pose.position.y
)
loc['coordinates'].append(xy_coord)
geojson['loc'] = loc
# time
geojson['start'] = msg.start_time.to_sec()
geojson['end'] = msg.end_time.to_sec()
start_dt = datetime.fromtimestamp(msg.start_time.to_sec())
geojson['start_weekday'] = start_dt.weekday()
geojson['start_hour'] = start_dt.hour
geojson['start_minute'] = start_dt.minute
end_dt = datetime.fromtimestamp(msg.end_time.to_sec())
geojson['end_weekday'] = end_dt.weekday()
geojson['end_hour'] = end_dt.hour
geojson['end_minute'] = end_dt.minute
return geojson
class TrajectoryImporter(object):
def __init__(self, online, traj_topic):
self._traj = dict()
rospy.loginfo("Connecting to mongodb...")
self.gs = GeoSpatialStoreProxy('geospatial_store', 'soma')
if online:
if traj_topic[0] != '/':
traj_topic = '/' + traj_topic
if traj_topic[-1] == '/':
traj_topic = traj_topic[:-1]
rospy.loginfo("Subscribing to %s..." % traj_topic)
rospy.Subscriber(
"%s/complete" % traj_topic, Trajectories,
self.traj_callback, None, queue_size=10
)
else:
self._traj = dict()
trajs = OfflineTrajectories()
for uuid, traj in trajs.traj.iteritems():
self._traj[uuid] = traj.get_trajectory_message()
def store_all(self):
for uuid, t in self._traj.items():
geo_json = self.geojson_from_trajectory(t)
# in case trajectory is already in there => replace
ids = self.gs.find_projection({"uuid": uuid}, {"_id": 1})
for _id in ids:
self.gs.remove(_id["_id"])
rospy.loginfo("Storing %s data to geospatial_store", uuid)
self.gs.insert(geo_json)
# meta = dict()
# meta["map"] = map
# self._store_client.insert(msg, meta)
del self._traj[uuid]
rospy.sleep(0.5)
def traj_callback(self, msg):
self._traj.update({
traj.uuid: traj for i, traj in enumerate(msg.trajectories)
})
def geojson_from_trajectory(self, msg):
geojson = {}
# trajectory UUID
geojson['uuid'] = msg.uuid
# space
loc = {}
loc['type'] = 'LineString'
loc['coordinates'] = []
for ps in msg.trajectory:
xy_coord = self.gs.coords_to_lnglat(
ps.pose.position.x, ps.pose.position.y
)
loc['coordinates'].append(xy_coord)
geojson['loc'] = loc
# time
geojson['start'] = msg.start_time.to_sec()
geojson['end'] = msg.end_time.to_sec()
start_dt = datetime.fromtimestamp(msg.start_time.to_sec())
geojson['start_weekday'] = start_dt.weekday()
geojson['start_hour'] = start_dt.hour
geojson['start_minute'] = start_dt.minute
end_dt = datetime.fromtimestamp(msg.end_time.to_sec())
geojson['end_weekday'] = end_dt.weekday()
geojson['end_hour'] = end_dt.hour
geojson['end_minute'] = end_dt.minute
return geojson
class SOMAROIManager():
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()
def _init_types(self):
# read from config in soma_objects
with open(self._config_file) as config_file:
config = json.load(config_file)
self.mesh = dict()
for k, v in config['roi'].iteritems():
self.mesh[k] = v
def _init_menu(self):
self.menu_handler = MenuHandler()
add_point_entry = self.menu_handler.insert("Add Point",
callback=self._add_point_cb)
del_point_entry = self.menu_handler.insert("Delete Point",
callback=self._del_point_cb)
add_entry = self.menu_handler.insert("Add ROI")
self.menu_item = dict()
for k in sorted(self.mesh.keys()):
entry = self.menu_handler.insert(k,
parent=add_entry,
callback=self._add_cb)
self.menu_item[entry] = k
del_entry = self.menu_handler.insert("Delete ROI",
callback=self._del_cb)
enable_entry = self.menu_handler.insert("Movement control",
callback=self._enable_cb)
self.menu_handler.setCheckState(enable_entry, MenuHandler.CHECKED)
def _add_cb(self, feedback):
rospy.loginfo("Add ROI: %s", self.menu_item[feedback.menu_entry_id])
self.add_object(self.menu_item[feedback.menu_entry_id], feedback.pose)
def _del_cb(self, feedback):
rospy.loginfo("Delete ROI: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
for r in self._soma_obj_roi_ids[roi]:
self.delete_object(r)
self.undraw_roi(roi)
def _add_point_cb(self, feedback):
rospy.loginfo("Add point: %s", feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
t = self._soma_obj_type[feedback.marker_name]
self.add_object(t, feedback.pose, roi)
#self.draw_roi(roi)
def _del_point_cb(self, feedback):
rospy.loginfo("Delete point: %s", feedback.marker_name)
self.delete_object(feedback.marker_name)
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
def _update_poly(self, feedback):
return
def _update_cb(self, feedback):
p = feedback.pose.position
#print "Marker " + feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y)
self._soma_obj_pose[feedback.marker_name] = feedback.pose
roi = self._soma_obj_roi[feedback.marker_name]
self.draw_roi(roi)
if hasattr(self, "vp_timer_" + feedback.marker_name):
getattr(self, "vp_timer_" + feedback.marker_name).cancel()
setattr(self, "vp_timer_" + feedback.marker_name,
Timer(3, self.update_object, [feedback]))
getattr(self, "vp_timer_" + feedback.marker_name).start()
def _enable_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
if state == MenuHandler.CHECKED:
self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
self._interactive = False
else:
self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
self._interactive = True
self.menu_handler.reApply(self._server)
self.load_objects()
self._server.applyChanges()
def _next_id(self):
self._soma_id += 1
return self._soma_id
def _next_roi_id(self):
self._soma_roi_id += 1
return self._soma_roi_id
def _retrieve_objects(self):
objs = self._msg_store.query(SOMAROIObject._type,
message_query={
"map": self.soma_map,
"config": self.soma_conf
})
max_id = 0
max_roi_id = 0
for o, om in objs:
if int(o.id) > max_id:
max_id = int(o.id)
if int(o.roi_id) > max_roi_id:
max_roi_id = int(o.roi_id)
self._soma_id = max_id
self._soma_roi_id = max_roi_id
return objs
def load_objects(self):
self._soma_obj_roi_ids = dict()
objs = self._retrieve_objects()
# if collection is empty insert initial object
if not objs:
pose = Pose()
self.add_object('Office', pose)
return
# otherwise, load all object from collection
for o, om in objs:
self._soma_obj_ids[o.id] = om['_id']
self._soma_obj_msg[o.id] = o
if o.roi_id in self._soma_obj_roi_ids:
self._soma_obj_roi_ids[o.roi_id].append(o.id)
else:
self._soma_obj_roi_ids[o.roi_id] = list()
self._soma_obj_roi_ids[o.roi_id].append(o.id)
self._soma_obj_roi[o.id] = o.roi_id
self._soma_obj_type[o.id] = o.type
self._soma_obj_pose[o.id] = o.pose
self.load_object(o.id, o.roi_id, o.type, o.pose)
self.draw_all_roi()
def draw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.draw_roi(key)
def undraw_all_roi(self):
for key in self._soma_obj_roi_ids:
self.undraw_roi(key)
def draw_roi(self, roi):
v = self._soma_obj_roi_ids[roi]
t = self._soma_obj_type[v[0]]
p = self._soma_obj_pose[v[0]]
int_marker = self.create_roi_marker(roi, t, p, v)
self._server.erase("ROI-" + roi)
self._server.applyChanges()
self._server.insert(int_marker, self._update_poly)
self._server.applyChanges()
def undraw_roi(self, roi):
self._server.erase("ROI-" + roi)
self._server.applyChanges()
def load_object(self, soma_id, roi, soma_type, pose):
int_marker = self.create_object_marker(soma_id, roi, soma_type, pose)
self._server.insert(int_marker, self._update_cb)
self.menu_handler.apply(self._server, soma_id)
self._server.applyChanges()
def add_object(self, soma_type, pose, roi_id=None):
# todo: add to mongodb
soma_id = self._next_id()
if roi_id == None:
soma_roi_id = self._next_roi_id()
self._soma_obj_roi_ids[str(soma_roi_id)] = list()
else:
soma_roi_id = roi_id
soma_obj = SOMAROIObject()
soma_obj.id = str(soma_id)
soma_obj.roi_id = str(soma_roi_id)
soma_obj.map = str(self.soma_map)
soma_obj.config = str(self.soma_conf)
soma_obj.type = soma_type
soma_obj.pose = pose
soma_obj.frame = 'map'
_id = self._msg_store.insert(soma_obj)
self._soma_obj_ids[soma_obj.id] = _id
self._soma_obj_msg[soma_obj.id] = soma_obj
self._soma_obj_roi_ids[soma_obj.roi_id].append(soma_obj.id)
self._soma_obj_roi[soma_obj.id] = soma_obj.roi_id
self._soma_obj_type[soma_obj.id] = soma_type
self._soma_obj_pose[soma_obj.id] = pose
self.load_object(str(soma_id), soma_obj.roi_id, soma_type, pose)
# geospatial store
# delete old message
res = self._gs_store.find_one({
'soma_roi_id': soma_roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(soma_obj)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(soma_obj.type, soma_obj.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (soma_type, soma_roi_id))
def geo_json_from_soma_obj(self, soma_obj):
geo_json = {}
geo_json['soma_roi_id'] = soma_obj.roi_id
geo_json['soma_map'] = soma_obj.map
geo_json['soma_config'] = soma_obj.config
geo_json['type'] = soma_obj.type
if len(self._soma_obj_roi_ids[soma_obj.roi_id]) < 3:
rospy.logerr(
"GS Store: %s %s, less then 3 points => Add more points or delete ROI."
% (soma_obj.type, soma_obj.roi_id))
return None
coordinates = []
for obj_id in self._soma_obj_roi_ids[soma_obj.roi_id]:
p = self._soma_obj_pose[obj_id]
coordinates.append(
self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
p = self._soma_obj_pose[self._soma_obj_roi_ids[soma_obj.roi_id][0]]
coordinates.append(
self._gs_store.coords_to_lnglat(p.position.x, p.position.y))
geo_json['loc'] = {'type': 'Polygon', 'coordinates': [coordinates]}
return geo_json
def delete_object(self, soma_id):
# todo: delete from mongodb
_id = self._soma_obj_ids[str(soma_id)]
self._msg_store.delete(str(_id))
self._server.erase(soma_id)
self._server.applyChanges()
roi = self._soma_obj_roi[str(soma_id)]
nodes = self._soma_obj_roi_ids[roi]
new_nodes = []
for n in nodes:
if n != str(soma_id):
new_nodes.append(n)
self._soma_obj_roi_ids[roi] = new_nodes
# geospatial store
# delete old message
old_msg = self._soma_obj_msg[soma_id]
res = self._gs_store.find_one({
'soma_roi_id': roi,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(old_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(old_msg.type, old_msg.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (old_msg.type, old_msg.roi_id))
def update_object(self, feedback):
rospy.loginfo("Updated marker: %s", feedback.marker_name)
_id = self._soma_obj_ids[feedback.marker_name]
msg = self._soma_obj_msg[feedback.marker_name]
new_msg = copy.deepcopy(msg)
new_msg.pose = feedback.pose
self._soma_obj_pose[feedback.marker_name] = feedback.pose
self._msg_store.update_id(_id, new_msg)
# geospatial store
# delete old message
res = self._gs_store.find_one({
'soma_roi_id': new_msg.roi_id,
'soma_map': self.soma_map,
'soma_config': self.soma_conf
})
if res:
_gs_id = res['_id']
self._gs_store.remove(_gs_id)
#rospy.loginfo("GS Store: deleted roi")
# add new object to geospatial store
geo_json = self.geo_json_from_soma_obj(new_msg)
if geo_json:
try:
self._gs_store.insert(geo_json)
rospy.loginfo("GS Store: updated roi (%s %s)" %
(new_msg.type, new_msg.roi_id))
except:
rospy.logerr(
"The polygon of %s %s is malformed (self-intersecting) => Please update geometry."
% (new_msg.type, new_msg.roi_id))
def create_object_marker(self, soma_obj, roi, soma_type, pose):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = soma_obj
int_marker.description = soma_type + ' (' + roi + ')'
int_marker.pose = pose
int_marker.pose.position.z = 0.01
marker = Marker()
marker.type = Marker.SPHERE
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
int_marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
if self._interactive:
int_marker.controls.append(copy.deepcopy(control))
# add the control to the interactive marker
int_marker.controls.append(control)
# add menu control
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
menu_control.markers.append(marker) #makeBox(int_marker) )
int_marker.controls.append(menu_control)
return int_marker
def create_roi_marker(self, roi, soma_type, pose, points):
#print "POINTS: " + str(points)
int_marker = InteractiveMarker()
int_marker.header.frame_id = "map"
int_marker.name = "ROI-" + roi
int_marker.description = roi
int_marker.pose = pose
marker = Marker()
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(marker)
int_marker.controls.append(control)
marker.points = []
for point in points:
p = Point()
pose = self._soma_obj_pose[point]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
p = Point()
pose = self._soma_obj_pose[points[0]]
p.x = pose.position.x - int_marker.pose.position.x
p.y = pose.position.y - int_marker.pose.position.y
marker.points.append(p)
return int_marker
