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 test_non_ascii(self):
msg_store = MessageStoreProxy()
msg = String(data="こんにちは") # non ascii string
doc_id = msg_store.insert(msg)
try:
qmsg, _ = msg_store.query_id(doc_id, String._type)
self.assertEqual(msg.data, qmsg.data)
except rospy.service.ServiceException:
self.fail("non ascii unicode string cannot be queried")
class PeopleCounter(object):
def __init__(self, config, region_categories=dict(), coll='activity_robblog'):
rospy.loginfo("Starting activity checking...")
self.region_categories = region_categories
self._lock = False
# regions = {roi:polygon} and soma map info
self.regions, self.soma_map = get_soma_info(config)
self.reset()
# tf stuff
self._tfl = tf.TransformListener()
# create db
rospy.loginfo("Create database collection %s..." % coll)
self._db = MessageStoreProxy(collection=coll)
self._db_image = MessageStoreProxy(collection=coll+"_img")
self._ubd_db = MessageStoreProxy(collection="upper_bodies")
# service client to upper body logging
rospy.loginfo("Create client to /vision_logging_service/capture...")
self.capture_srv = rospy.ServiceProxy(
"/vision_logging_service/capture", CaptureUBD
)
# subscribing to ubd topic
subs = [
message_filters.Subscriber(
rospy.get_param(
"~ubd_topic", "/upper_body_detector/bounding_box_centres"
),
PoseArray
),
message_filters.Subscriber(
rospy.get_param("~tracker_topic", "/people_tracker/positions"),
PeopleTracker
)
]
ts = message_filters.ApproximateTimeSynchronizer(
subs, queue_size=5, slop=0.15
)
ts.registerCallback(self.cb)
def reset(self):
# start modified code
self.detected_time = dict()
# end modified code
self.uuids = {roi: list() for roi, _ in self.regions.iteritems()}
self.image_ids = {roi: list() for roi, _ in self.regions.iteritems()}
self.people_poses = list()
self._stop = False
self._ubd_pos = list()
self._tracker_pos = list()
self._tracker_uuids = list()
def cb(self, ubd_cent, pt):
if not self._lock:
self._lock = True
self._tracker_uuids = pt.uuids
self._ubd_pos = self.to_world_all(ubd_cent)
self._tracker_pos = [i for i in pt.poses]
self._lock = False
def to_world_all(self, pose_arr):
transformed_pose_arr = list()
try:
fid = pose_arr.header.frame_id
for cpose in pose_arr.poses:
ctime = self._tfl.getLatestCommonTime(fid, "/map")
pose_stamped = PoseStamped(Header(1, ctime, fid), cpose)
# Get the translation for this camera's frame to the world.
# And apply it to all current detections.
tpose = self._tfl.transformPose("/map", pose_stamped)
transformed_pose_arr.append(tpose.pose)
except tf.Exception as e:
rospy.logwarn(e)
# In case of a problem, just give empty world coordinates.
return []
return transformed_pose_arr
def stop_check(self):
self._stop = True
def _is_new_person(self, ubd_pose, track_pose, tracker_ind):
pose_inside_roi = ''
# merge ubd with tracker pose
cond = euclidean(
[ubd_pose.position.x, ubd_pose.position.y],
[track_pose.position.x, track_pose.position.y]
) < 0.2
# uuid must be new
if cond:
is_new = True
for roi, uuids in self.uuids.iteritems():
if self._tracker_uuids[tracker_ind] in uuids:
is_new = False
break
cond = cond and is_new
if cond:
# this pose must be inside a region
for roi, region in self.regions.iteritems():
if region.contains(
Point(ubd_pose.position.x, ubd_pose.position.y)
):
pose_inside_roi = roi
break
cond = cond and (pose_inside_roi != '')
if cond:
is_near = False
for pose in self.people_poses:
if euclidean(
pose, [ubd_pose.position.x, ubd_pose.position.y]
) < 0.3:
is_near = True
break
cond = cond and (not is_near)
return cond, pose_inside_roi
def _uuids_roi_to_category(self, dict_uuids):
result = dict()
for roi, uuids in dict_uuids.iteritems():
region_category = roi
if region_category in self.region_categories:
region_category = self.region_categories[region_category]
if region_category not in result:
result[region_category] = (list(), list())
result[region_category][0].append(roi)
result[region_category][1].extend(uuids)
return result
def _create_robmsg(self, start_time, end_time):
regions_to_string = dict()
regions_to_string_img = dict()
for region_category, (rois, uuids) in self._uuids_roi_to_category(self.uuids).iteritems():
if region_category not in regions_to_string:
regions_to_string[region_category] = '# Activity Report \n'
regions_to_string[region_category] += ' * **Regions:** %s \n' % str(rois)
regions_to_string[region_category] += ' * **Area:** %s \n' % region_category
regions_to_string[region_category] += ' * **Start time:** %s \n' % str(start_time)
regions_to_string[region_category] += ' * **End time:** %s \n' % str(end_time)
regions_to_string[region_category] += ' * **Summary:** %d person(s) were detected \n' % len(uuids)
regions_to_string[region_category] += ' * **Details:** \n\n'
regions_to_string_img[region_category] = copy.copy(regions_to_string[region_category])
for roi in rois:
for ind, uuid in enumerate(self.uuids[roi]):
try:
detected_time = self.detected_time[uuid]
except:
detected_time = start_time
detected_time = datetime.datetime.fromtimestamp(detected_time.secs)
regions_to_string[region_category] += '%s was detected at %s \n\n' % (uuid, detected_time)
regions_to_string_img[region_category] += ") " % (
uuid, self.image_ids[roi][ind]
)
regions_to_string_img[region_category] += 'was detected at %s \n\n' % str(detected_time)
entries = list()
for region_category, string_body in regions_to_string.iteritems():
entries.append(RobblogEntry(
title="%s Activity Report - %s" % (start_time.date(), region_category),
body=string_body
))
entry_images = list()
for region_category, string_body in regions_to_string_img.iteritems():
entry_images.append(RobblogEntry(
title="%s Activity Report - %s" % (start_time.date(), region_category),
body=string_body
))
return entries, entry_images
def _store(self, start_time, end_time):
rospy.loginfo("Storing location and the number of detected persons...")
start_time = datetime.datetime.fromtimestamp(start_time.secs)
end_time = datetime.datetime.fromtimestamp(end_time.secs)
entries, entry_images = self._create_robmsg(start_time, end_time)
for entry in entries:
self._db.insert(entry)
for entry in entry_images:
self._db_image.insert(entry)
def continuous_check(self, duration):
rospy.loginfo("Start looking for people...")
start_time = rospy.Time.now()
end_time = rospy.Time.now()
while (end_time - start_time) < duration and not self._stop:
if not self._lock:
self._lock = True
for ind_ubd, i in enumerate(self._ubd_pos):
for ind, j in enumerate(self._tracker_pos):
cond, pose_inside_roi = self._is_new_person(i, j, ind)
if cond:
result = self.capture_srv()
rospy.sleep(0.1)
_id = ""
if len(result.obj_ids) > 0:
ubd_log = self._ubd_db.query_id(
result.obj_ids[0], LoggingUBD._type
)
try:
_id = self._db_image.insert(
ubd_log[0].ubd_rgb[ind_ubd]
)
except:
rospy.logwarn(
"Missed the person to capture images..."
)
self.image_ids[pose_inside_roi].append(_id)
# self.uuids.append(self._tracker_uuids[ind])
self.uuids[pose_inside_roi].append(
self._tracker_uuids[ind]
)
if self._tracker_uuids[ind] not in self.detected_time:
self.detected_time[self._tracker_uuids[ind]] = end_time
self.people_poses.append(
[i.position.x, i.position.y]
)
rospy.loginfo(
"%s is detected in region %s - (%.2f, %.2f)" % (
self._tracker_uuids[ind], pose_inside_roi,
i.position.x, i.position.y
)
)
self._lock = False
end_time = rospy.Time.now()
rospy.sleep(0.1)
self._store(start_time, end_time)
self._stop = False
p_id = msg_store.insert(['test1', 'test2'])
# get it back with a name
print msg_store.query_named("my favourite pose", Pose._type)
p.position.x = 666
# update it with a name
msg_store.update_named("my favourite pose", p)
p.position.y = 2020
# update the other inserted one using the id
msg_store.update_id(p_id, p)
stored_p, meta = msg_store.query_id(p_id, Pose._type)
assert stored_p.position.x == 666
assert stored_p.position.y == 2020
print "stored object ok"
print "stored object inserted at %s (UTC rostime) by %s" % (
meta['inserted_at'], meta['inserted_by'])
print "stored object last updated at %s (UTC rostime) by %s" % (
meta['last_updated_at'], meta['last_updated_by'])
# some other things you can do...
# 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
class ActionExecutor(object):
def __init__(self):
self.msg_store = MessageStoreProxy()
self.cancelled = False
def get_action_types(self, action_server):
"""
Returns the type string related to the action string provided.
"""
rospy.logdebug("Action server provided: %s", action_server)
topics = rospy.get_published_topics(action_server)
for [topic, type] in topics:
if topic.endswith('feedback'):
return (type[:-8], type[:-14] + 'Goal')
raise RuntimeError('No action associated with topic: %s' %
action_server)
def instantiate_from_string_pair(self, string_pair):
if string_pair[0] == Task.STRING_TYPE:
return string_pair[1]
elif string_pair[0] == Task.INT_TYPE:
return int(string_pair[1])
elif string_pair[0] == Task.FLOAT_TYPE:
return float(string_pair[1])
elif string_pair[0] == Task.TIME_TYPE:
return rospy.Time.from_sec(float(string_pair[1]))
elif string_pair[0] == Task.DURATION_TYPE:
return rospy.Duration.from_sec(float(string_pair[1]))
elif string_pair[0] == Task.BOOL_TYPE:
return string_pair[1] == 'True'
else:
msg = self.msg_store.query_id(string_pair[1], string_pair[0])[0]
# print msg
if msg == None:
raise RuntimeError("No matching object for id %s of type %s" %
(string_pair[1], string_pair[0]))
return msg
def get_arguments(self, argument_list):
res = []
for string_pair in argument_list:
res.append(
self.instantiate_from_string_pair(
[string_pair.first, string_pair.second]))
return res
def get_max_action_duration(self, action_outcomes):
res = 0
for action_outcome_msg in action_outcomes:
for duration in action_outcome_msg.durations:
res = max(res, duration)
return res
def get_max_prob_outcome(self, action_outcomes):
max_prob = 0
res = None
for action_outcome_msg in action_outcomes:
if action_outcome_msg.probability > max_prob:
max_prob = action_outcome_msg.probability
res = action_outcome_msg
return res
def is_action_interruptible(self, action):
if action is None:
rospy.logwarn(
'is_action_interruptible passed a None, returning True')
return True
try:
srv_name = action + '_is_interruptible'
rospy.wait_for_service(srv_name, timeout=1)
is_interruptible = rospy.ServiceProxy(srv_name,
IsTaskInterruptible)
return is_interruptible().status
except rospy.ROSException as exc:
rospy.logdebug(
'%s service does not exist, treating as interruptible')
return True
except rospy.ServiceException as exc:
rospy.logwarn(exc.message)
return True
def execute_action(self, action_msg):
self.cancelled = False
rospy.loginfo("Executing " + action_msg.name +
". Actionlib server is: " + action_msg.action_server)
if action_msg.action_server != '':
try:
(action_string,
goal_string) = self.get_action_types(action_msg.action_server)
action_clz = dc_util.load_class(
dc_util.type_to_class_string(action_string))
goal_clz = dc_util.load_class(
dc_util.type_to_class_string(goal_string))
argument_list = self.get_arguments(action_msg.arguments)
goal = goal_clz(*argument_list)
poll_wait = rospy.Duration(1)
if action_msg.max_duration == rospy.Duration(0):
max_action_duration = self.get_max_action_duration(
action_msg.outcomes)
else:
max_action_duration = action_msg.max_duration.to_sec()
wiggle_room = 30
action_client = actionlib.SimpleActionClient(
action_msg.action_server, action_clz)
action_client.wait_for_server(poll_wait)
action_client.send_goal(goal)
action_finished = False
timer = 0
while (not action_finished) and (not self.cancelled):
timer += poll_wait.to_sec()
action_finished = action_client.wait_for_result(poll_wait)
if timer > (max_action_duration + wiggle_room):
if self.is_action_interruptible(
action_msg.action_server):
break
else:
rospy.logwarn(
'Action execution has timed out but is not interruptible, so execution continues'
)
if not action_finished:
if self.cancelled:
rospy.logwarn(
"Policy execution has been preempted by another process"
)
else:
rospy.logwarn(
'Action %s exceeded maximum duration, preempting' %
action_msg.name)
action_client.cancel_all_goals()
action_finished = action_client.wait_for_result(
rospy.Duration(
60)) #give some time for action to cancel
if not action_finished:
rospy.logwarn(
'Action %s did not respond to preemption, carrying on regardless. This could have dangerous side effects.'
% action_msg.name)
status = action_client.get_state()
result = action_client.get_result()
print GoalStatus.to_string(status)
print result
except Exception, e:
rospy.logwarn(
'Caught exception when trying to execute the action: %s' %
e)
status = GoalStatus.ABORTED
result = None
else:
# now store ids togther in store, addition types for safety
spl = StringPairList()
for pair in stored:
spl.pairs.append(StringPair(pair[0], pair[1]))
# and add some meta information
meta = {}
meta['description'] = "this wasn't great"
meta['result_time'] = datetime.utcfromtimestamp(rospy.get_rostime().to_sec())
msg_store.insert(spl, meta = meta)
# now let's get all our logged data back
results = msg_store.query(StringPairList._type)
for message, meta in results:
if 'description' in meta:
print 'description: %s' % meta['description']
print 'result time (UTC from rostime): %s' % meta['result_time']
print 'inserted at (UTC from rostime): %s' % meta['inserted_at']
pose = msg_store.query_id(message.pairs[0].second, Pose._type)
point = msg_store.query_id(message.pairs[1].second, Point._type)
quaternion = msg_store.query_id(message.pairs[2].second, Quaternion._type)
result = msg_store.query_id(message.pairs[3].second, Bool._type)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
stored.append([quaternion._type, msg_store.insert(quaternion)])
stored.append([result._type, msg_store.insert(result)])
# now store ids togther in store, addition types for safety
spl = StringPairList()
for pair in stored:
spl.pairs.append(StringPair(pair[0], pair[1]))
# and add some meta information
meta = {}
meta['description'] = "this wasn't great"
meta['result_time'] = datetime.utcfromtimestamp(
rospy.get_rostime().to_sec())
msg_store.insert(spl, meta=meta)
# now let's get all our logged data back
results = msg_store.query(StringPairList._type)
for message, meta in results:
if 'description' in meta:
print 'description: %s' % meta['description']
print 'result time (UTC from rostime): %s' % meta['result_time']
print 'inserted at (UTC from rostime): %s' % meta['inserted_at']
pose = msg_store.query_id(message.pairs[0].second, Pose._type)
point = msg_store.query_id(message.pairs[1].second, Point._type)
quaternion = msg_store.query_id(message.pairs[2].second,
Quaternion._type)
result = msg_store.query_id(message.pairs[3].second, Bool._type)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
p_id = msg_store.insert(['test1', 'test2'])
# get it back with a name
print msg_store.query_named("my favourite pose", Pose._type)
p.position.x = 666
# update it with a name
msg_store.update_named("my favourite pose", p)
p.position.y = 2020
# update the other inserted one using the id
msg_store.update_id(p_id, p)
stored_p, meta = msg_store.query_id(p_id, Pose._type)
assert stored_p.position.x == 666
assert stored_p.position.y == 2020
print "stored object ok"
print "stored object inserted at %s (UTC rostime) by %s" % (meta['inserted_at'], meta['inserted_by'])
print "stored object last updated at %s (UTC rostime) by %s" % (meta['last_updated_at'], meta['last_updated_by'])
# some other things you can do...
# 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)
