Refactor to make Facts public functions easier to understand.
This commit is contained in:
parent
67bbd8f1b9
commit
b2e862d4ac
|
|
@ -15,32 +15,26 @@ import os
|
||||||
import yaml
|
import yaml
|
||||||
from ament_index_python import get_package_share_directory
|
from ament_index_python import get_package_share_directory
|
||||||
|
|
||||||
|
from am_i_up.Ros import Ros
|
||||||
|
|
||||||
class Facts:
|
class Facts:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
self._ros = Ros()
|
||||||
|
|
||||||
self._start_time = time.monotonic()
|
self._start_time = time.monotonic()
|
||||||
self._status_string = None
|
self._status_string = None
|
||||||
self._costmap_image_format = None
|
|
||||||
self._costmap_image_data = None
|
|
||||||
self._lat_lon = (None, None)
|
|
||||||
|
|
||||||
def set_lat_long(self, lat, lon):
|
async def run(self):
|
||||||
self._lat_lon = (lat, lon)
|
await self._ros.run()
|
||||||
|
|
||||||
def get_lat_long(self):
|
def get_lat_long(self):
|
||||||
return self._lat_lon
|
return self._ros.get_lat_long()
|
||||||
|
|
||||||
def set_costmap_image(self, image_format, image_data):
|
|
||||||
self._costmap_image_format = image_format
|
|
||||||
self._costmap_image_data = image_data
|
|
||||||
|
|
||||||
def get_costmap_image(self):
|
def get_costmap_image(self):
|
||||||
return (self._costmap_image_format, self._costmap_image_data)
|
return self._ros.get_costmap_image()
|
||||||
|
|
||||||
def set_status_string(self, status):
|
|
||||||
self._status_string = status
|
|
||||||
|
|
||||||
def get_status(self):
|
def get_status(self):
|
||||||
return self._status_string
|
return self._ros.get_status_string()
|
||||||
|
|
||||||
def get_uptime(self):
|
def get_uptime(self):
|
||||||
return time.monotonic() - self._start_time
|
return time.monotonic() - self._start_time
|
||||||
|
|
|
||||||
|
|
@ -15,29 +15,41 @@ from sensor_msgs.msg import NavSatFix
|
||||||
import asyncio
|
import asyncio
|
||||||
|
|
||||||
class Ros:
|
class Ros:
|
||||||
def __init__(self, facts):
|
def __init__(self):
|
||||||
rclpy.init()
|
rclpy.init()
|
||||||
self._facts = facts
|
|
||||||
self._node = rclpy.create_node('am_i_up')
|
self._node = rclpy.create_node('am_i_up')
|
||||||
|
|
||||||
self._node.create_subscription(String, "status", self.status_sub, 1)
|
self._status_sub = self._node.create_subscription(String, "status", self._status_callback, 1)
|
||||||
self._node.create_subscription(CompressedImage, "image/compressed", self.image_sub, 1)
|
self._image_sub = self._node.create_subscription(CompressedImage, "image/compressed", self._image_callback, 1)
|
||||||
self._node.create_subscription(NavSatFix, "navsatfix", self.navsat_sub, 1)
|
self._navsat_sub = self._node.create_subscription(NavSatFix, "navsatfix", self._navsat_callback, 1)
|
||||||
|
|
||||||
def status_sub(self, msg):
|
self._status_string = None
|
||||||
self._facts.set_status_string(msg.data)
|
self._lat_long = None
|
||||||
|
self._costmap_image = None
|
||||||
|
|
||||||
def navsat_sub(self, msg):
|
def get_status_string(self):
|
||||||
self._facts.set_lat_long(msg.latitude, msg.longitude)
|
return self._status_string
|
||||||
|
|
||||||
def image_sub(self, msg):
|
def get_lat_long(self):
|
||||||
if msg.format != "jpg" and msg.format != "png":
|
return self._lat_long
|
||||||
# I don't know what to do with this image....
|
|
||||||
return
|
def get_costmap_image(self):
|
||||||
self._facts.set_costmap_image(msg.format, bytes(msg.data))
|
return self._costmap_image
|
||||||
|
|
||||||
async def run(self):
|
async def run(self):
|
||||||
while rclpy.ok():
|
while rclpy.ok():
|
||||||
rclpy.spin_once(self._node, timeout_sec=0)
|
rclpy.spin_once(self._node, timeout_sec=0)
|
||||||
await asyncio.sleep(1e-4)
|
await asyncio.sleep(1e-4)
|
||||||
|
|
||||||
|
def _status_callback(self, msg):
|
||||||
|
self._status_string = msg.data
|
||||||
|
|
||||||
|
def _navsat_callback(self, msg):
|
||||||
|
self._lat_long = (msg.latitude, msg.longitude)
|
||||||
|
|
||||||
|
def _image_callback(self, msg):
|
||||||
|
if msg.format != "jpg" and msg.format != "png":
|
||||||
|
# I don't know what to do with this image....
|
||||||
|
return
|
||||||
|
self._costmap_image = (msg.format, bytes(msg.data))
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -10,14 +10,12 @@
|
||||||
#
|
#
|
||||||
from am_i_up.Api import Api
|
from am_i_up.Api import Api
|
||||||
from am_i_up.Facts import Facts
|
from am_i_up.Facts import Facts
|
||||||
from am_i_up.Ros import Ros
|
|
||||||
|
|
||||||
import asyncio
|
import asyncio
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
facts = Facts()
|
facts = Facts()
|
||||||
ros = Ros(facts)
|
|
||||||
api = Api(facts)
|
api = Api(facts)
|
||||||
|
|
||||||
future = asyncio.gather(api.run(), ros.run())
|
future = asyncio.gather(api.run(), facts.run())
|
||||||
asyncio.get_event_loop().run_until_complete(future)
|
asyncio.get_event_loop().run_until_complete(future)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue