class PowerMonitor:
onDockChanged = mayfield_utils.Event()
onChargingChanged = mayfield_utils.Event()
def __init__(self, dock_changed_cb=None, charging_changed_cb=None):
self._on_dock = False
self._charging = False
if dock_changed_cb:
self.onDockChanged.connect(dock_changed_cb)
if charging_changed_cb:
self.onChargingChanged.connect(charging_changed_cb)
self._power_sub = rospy.Subscriber("mobile_base/power",
mobile_base_driver.msg.Power,
self._power_cb)
def shutdown(self):
self._power_sub.unregister()
def _power_cb(self, msg):
if self._on_dock != msg.dock_present:
self._on_dock = msg.dock_present
self.onDockChanged(msg)
if self._charging != msg.is_charging:
self._charging = msg.is_charging
self.onChargingChanged(msg)
class OortMapManager:
onMapGrew = mayfield_utils.Event()
def __init__(self):
self._map_path = None
self._map_size = 0
self._graph_loc_create_srv = rospy.ServiceProxy(
"oort_ros_mapping/graph_loc/create", oort_msgs.srv.LocCreate)
self._graph_loc_list_srv = rospy.ServiceProxy(
"oort_ros_mapping/graph_loc/list", oort_msgs.srv.LocNamespace)
self._graph_loc_locate_srv = rospy.ServiceProxy(
"oort_ros_mapping/graph_loc/locate", oort_msgs.srv.LocLocate)
self._map_load_srv = rospy.ServiceProxy("oort_ros_mapping/map/load",
oort_msgs.srv.SetString)
self._map_republish_srv = rospy.ServiceProxy(
"oort_ros_mapping/map/republish_maps", std_srvs.srv.Empty)
self._mapping_state_srv = rospy.ServiceProxy(
"oort_ros_mapping/map/state", oort_msgs.srv.GetString)
self._mapping_start_srv = rospy.ServiceProxy(
"oort_ros_mapping/map/start", oort_msgs.srv.SetString)
self._mapping_stop_srv = rospy.ServiceProxy(
"oort_ros_mapping/map/stop", std_srvs.srv.Empty)
self._notify_dock_srv = rospy.ServiceProxy(
"oort_ros_mapping/map/notify_docked", std_srvs.srv.Empty)
self._amcl_start_srv = rospy.ServiceProxy("localization_start",
std_srvs.srv.Empty)
self._relocalization_pub = rospy.Publisher("initialpose_cloud",
amcl.msg.HypothesisSet,
queue_size=1)
self._map_sub = rospy.Subscriber("map", nav_msgs.msg.OccupancyGrid,
self._map_cb)
def convert_map(self):
assert self._map_path, "Mapping not started. No map to convert"
subprocess.check_call(["rosrun", "map_server", "map_saver"],
cwd=self._map_path)
def get_map_state(self):
return self._mapping_state_srv().data
def get_dock_pose(self):
'''
Get the pose of the dock.
Return [x, y, theta] in map frame.
'''
dock_names = self._graph_loc_list_srv("dock", False)
# Really, there's only one dock and it has a reserved name, so we
# expect the following:
# dock_names.nspaces to be ["dock"]
if not dock_names:
# Early out if for some reason there are no docks
return None
uuid = dock_names.ids[0]
pose_stamped = self._graph_loc_locate_srv(nspace="dock", id=uuid).pose
if pose_stamped is None:
return None
return pose_to_se2(pose_stamped.pose)
def load_map(self, map_path):
'''
Loads an existing OORT map
:param map_path: A full path to the map.map_map_capnp file to load
'''
# Need to resolve simlinks, or OORT will look for the md5 in the wrong
# place. We also need to remove the file extension before we give the
# path to OORT
map_path = os.path.realpath(map_path)
if ".map_capnp" in map_path:
map_path = os.path.splitext(map_path)[0]
self._map_load_srv(map_path)
def localize_on_dock(self):
'''
Tells AMCL that Kuri is on the dock
'''
# Start with the dock pose from OORT
dock_pose = self.get_dock_pose()
# Convert to something that the relocalization_pub understands
hset = amcl.msg.HypothesisSet(hypotheses=[
_pose_to_posecov(se2_to_pose(dock_pose), (.15, .15,
math.radians(25)))
])
self._relocalization_pub.publish(hset)
def notify_docked(self):
self._notify_dock_srv()
def save_waypoint(self, nspace, wp_name):
self._graph_loc_create_srv(nspace, wp_name)
def start_localization(self):
'''
Starts AMCL
This method expects OORT to have a map. Either loaded from disk, or
created
'''
# Make sure we're not actively mapping, or both OORT and AMCL are
# going to try to publish odom
assert self.get_map_state() != "mapping"
self._amcl_start_srv()
# AMCL will use the first map it sees after it's started:
self._map_republish_srv()
def start_mapping(self):
'''
Starts OORT mapping and returns the full path to the saved map data
:returns: A path to the directory used to store the map
'''
map_path = os.path.join(os.path.expanduser("~"), "oort_maps",
str(uuid.uuid4()))
os.makedirs(map_path) # Recursive
# OORT node will drop a few files in the map_path directory:
# - map.map_capnp
# - map.map_meta_capnp
# - map.md5
self._mapping_start_srv(os.path.join(map_path, "map"))
self._map_path = map_path
# OORT has a bug where it will start mapping, but it won't be ready
# to save waypoints because it doesn't have a pose estimate yet.
# This won't get fixed in OORT unfortunately, so we just need to give
# OORT some extra time to start
rospy.sleep(1.0)
return map_path
def stop_mapping(self):
self._mapping_stop_srv()
def shutdown(self):
self._mapping_state_srv.close()
self._mapping_start_srv.close()
self._mapping_stop_srv.close()
self._map_sub.unregister()
self._relocalization_pub.unregister()
def _map_cb(self, msg):
# Calculate map size in square meters.
pxl_area = msg.info.resolution * msg.info.resolution
map_size = sum(d == 0 for d in msg.data) * pxl_area
if map_size - self._map_size > 1:
self._map_size = map_size
self.onMapGrew(map_size)