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