def _get_waypoints(self, soma_config):
region_wps = dict()
# get regions information
regions, _ = get_soma_info(soma_config)
# get waypoint information
topo_sub = rospy.Subscriber("/topological_map", TopologicalMap,
self._topo_map_cb, None, 10)
rospy.loginfo("Getting information from /topological_map...")
while self.topo_map is None:
rospy.sleep(0.1)
topo_sub.unregister()
for wp in self.topo_map.nodes:
wp_sight, _ = robot_view_cone(wp.pose)
intersected_rois = list()
intersected_regions = list()
for roi, region in regions.iteritems():
if is_intersected(wp_sight, region):
intersected_regions.append(region)
intersected_rois.append(roi)
for ind, region in enumerate(intersected_regions):
area = wp_sight.intersection(region).area
roi = intersected_rois[ind]
if roi in region_wps:
_, area1 = region_wps[roi]
if area > area1:
region_wps.update({roi: (wp.name, area)})
else:
region_wps.update({roi: (wp.name, area)})
return {
roi: tupleoftwo[0]
for roi, tupleoftwo in region_wps.iteritems()
}
def __init__(self):
rospy.loginfo('Initiating idle state...')
smach.State.__init__(self,
outcomes=['patrol', 'observe', 'aborted'],
input_keys=['waypoint', 'roi'],
output_keys=["waypoint", 'roi'])
self.probability = rospy.get_param("~patrol_probability", 0.7)
self.idle_duration = rospy.Duration(
rospy.get_param("~idle_duration", 300))
self.observe_duration = rospy.Duration(
rospy.get_param("~observe_duration", 1200))
self._is_counting_visit = False
self._region_visits = dict()
self.regions, _ = get_soma_info(
rospy.get_param("~soma_config", "activity_exploration"))
self.region_wps = yaml.load(
open(
roslib.packages.get_pkg_dir('activity_exploration') +
'/config/region_to_wp.yaml', 'r'))
rospy.loginfo("Region to WayPoint relation: %s" % str(self.region_wps))
self.type_wps = yaml.load(
open(
roslib.packages.get_pkg_dir('activity_exploration') +
'/config/type_to_wp.yaml', 'r'))
rospy.loginfo("Type of actions to WayPoints: %s" % str(self.type_wps))
if self.probability < 1.0:
rospy.loginfo("Connecting to /arms/activity_rcmd_srv...")
self.recommender_srv = rospy.ServiceProxy(
"/arms/activity_rcmd_srv", GetExplorationTasks)
self.recommender_srv.wait_for_service()
rospy.loginfo(
"Subscribing to /people_trajectory/trajectories/batch...")
rospy.Subscriber("/people_trajectory/trajectories/batch", Trajectories,
self._pt_cb, None, 10)
def __init__(self, config):
self.config = config
self.regions, self.map = get_soma_info(config)
self.topo_map = None
self._get_waypoints()
self.topo_map = [wp.name for wp in self.topo_map.nodes]
self.activity = {
roi: {wp: {
"Present": 0,
"Absent": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.ubd = {
roi:
{wp: {
"TP": 0,
"FN": 0,
"FP": 0,
"TN": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.cd = {
roi:
{wp: {
"TP": 0,
"FN": 0,
"FP": 0,
"TN": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.leg = {
roi:
{wp: {
"TP": 0,
"FN": 0,
"FP": 0,
"TN": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self._db_store = MessageStoreProxy(collection="ubd_scene_accuracy")
self.load_accuracy()
def __init__(self, name, soma_config):
"""
Generate random activities, at random times, and random places
"""
rospy.loginfo("Initiating activity generator...")
self._stop = True
self._lock = threading.Lock()
self.available_activities = ["Eat", "Drink", "Other"]
self._counter_uuid = 1
self.scheduled_activities = list()
self._time_ahead = rospy.Duration(60)
self.regions = get_soma_info(soma_config)[0].keys()
# self.available_activities = ["Eat", "Drink", "Rest", "Work", "Other"]
self.act_srv = rospy.Service("%s/available_activities" % name,
ActivitiesSrv, self._cb)
rospy.sleep(0.1)
self.act_pub = rospy.Publisher("%s/activities" % name,
ActivitiesMsg,
queue_size=10)
rospy.sleep(0.1)
def __init__(self,
config,
path="/localhome/%s/Pictures" % getpass.getuser()):
self.path = path
self.regions, self.map = get_soma_info(config)
self.topo_map = None
self._get_waypoints()
self.topo_map = {
wp.name:
Polygon([[wp.pose.position.x + i.x, wp.pose.position.y + i.y]
for i in wp.verts])
for wp in self.topo_map.nodes
}
self._stop = False
self._img = Image()
self.activity = {
roi: {wp: {
"Present": 0,
"Absent": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.ubd = {
roi:
{wp: {
"TP": 0,
"FN": 0,
"FP": 0,
"TN": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.cd = {
roi:
{wp: {
"TP": 0,
"FN": 0,
"FP": 0,
"TN": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.leg = {
roi:
{wp: {
"TP": 0,
"FN": 0,
"FP": 0,
"TN": 0
}
for wp in self.topo_map}
for roi in self.regions.keys()
}
self.wp_history = dict()
self.roi_activity = {roi: list() for roi in self.regions.keys()}
self.roi_non_activity = {roi: list() for roi in self.regions.keys()}
self.roi_cd = {roi: list() for roi in self.regions.keys()}
# self.roi_non_cd = {roi: list() for roi in self.regions.keys()}
self.roi_ubd = {roi: list() for roi in self.regions.keys()}
# self.roi_non_ubd = {roi: list() for roi in self.regions.keys()}
self.roi_leg = {roi: list() for roi in self.regions.keys()}
# self.roi_non_leg = {roi: list() for roi in self.regions.keys()}
self._db = MessageStoreProxy(collection="ubd_scene_log")
self._db_store = MessageStoreProxy(collection="ubd_scene_accuracy")
self._db_change = MessageStoreProxy(collection="change_detection")
self._db_ubd = pymongo.MongoClient(
rospy.get_param("mongodb_host", "localhost"),
rospy.get_param("mongodb_port", 62345)).message_store.upper_bodies
self._trajvis = TrajectoryVisualisation(
rospy.get_name() + "/leg",
{"start_time.secs": rospy.Time.now().secs})
# visualisation stuff
self._cd = list()
self._cd_len = 0
self._ubd = list()
self._ubd_len = 0
self._robot_pos = list()
self._robot_pos_len = 0
self._pub = rospy.Publisher(rospy.get_name(), Image, queue_size=10)
self._pub_cd_marker = rospy.Publisher(rospy.get_name() + "/cd_marker",
MarkerArray,
queue_size=10)
self._pub_ubd_marker = rospy.Publisher(rospy.get_name() +
"/ubd_marker",
MarkerArray,
queue_size=10)
self._pub_robot_marker = rospy.Publisher(rospy.get_name() +
"/robot_marker",
MarkerArray,
queue_size=10)
self.load_accuracy()
rospy.Timer(rospy.Duration(0.1), self.pub_img)