class ImageServer(object):
def __init__(self):
self.setup_ros()
self.wait_for_dependencies()
self.loop()
def setup_ros(self):
# This runs all of the preceeding functions.
rospy.init_node('image_server', log_level=rospy.DEBUG)
self.setup_parameters()
self.setup_ticket()
self.setup_request_nodes()
self.setup_image_nodes()
self.setup_publishers()
self.setup_model_server_proxy()
rospy.loginfo(f"[{self.name.abv}] ROS Setup")
def setup_parameters(self):
#Create an instance of the parameters object.
self.name = NameManager()
self.logger = Logger(name_manager = self.name)
self.img_s_parameters = ParameterManager(
logger = self.logger,
name_manager = self.name)
# Add each of our parameters and indicate the rate we wish
# for the noze to run
self.img_s_parameters.add(
Parameter(
name="rate",
target=f"{self.name.name}/rate",
default=50,
dynamic=True,
name_manager = self.name,
logger=self.logger))
# Indicates that the parameter should be updated dynamically
# if it changes after the node has started running.
self.img_s_parameters.add(
Parameter(
name="updates",
target=f"{self.name.name}/dynamic",
default=True,
dynamic=True,
name_manager=self.name,
logger=self.logger))
def setup_ticket(self):
# Instantiates a ticket object which contain both
# the flags for the type of image requested, as well
# as the last image itself
self.ticket = Ticket()
rospy.loginfo(f"[{self.name.abv}] Ticket Setup")
def wait_for_dependencies(self):
# There is no functionality for the image server unless our
# camera module is online.
dependencies = [self.camera_node]
for package in dependencies:
rospy.loginfo(f"[{self.name.abv}] Waiting for {package} to come online.")
rospy.wait_for_message(package, Image)
rospy.loginfo(f"[{self.name.abv}] {package} Online.")
return
def setup_request_nodes(self):
# Subscribe to the image request message from the Pose
# Estimation node
rospy.Subscriber('image_request/POS',
Bool,
self.image_rqst_callback,
queue_size = 1
)
rospy.loginfo(f"[{self.name.abv}] Pose Detection Node Subscribed")
# Subscribe to the Image request message from the Scene
# Segmentation node
rospy.Subscriber('image_request/SEG',
Bool,
self.image_rqst_callback,
queue_size = 1
)
rospy.loginfo(f"[{self.name.abv}] Segmentation Node Subscribed")
# Subscribe to the image request message from the depth
# perception node
rospy.Subscriber('image_request/DPT',
Bool,
self.image_rqst_callback,
queue_size = 1
)
rospy.loginfo(f"[{self.name.abv}] Depth Perception Node Subscribed")
def setup_image_nodes(self):
# Create the holding container for an incoming image message.
self.image = Image()
# Subscribe to the camera node.
self.camera_node = "/inland_ir_cam/image"
rospy.Subscriber(self.camera_node,
Image,
self.image_callback,
queue_size=1
)
rospy.loginfo(f"[{self.name.abv}] Camera Subscribed")
def setup_publishers(self):
# This is the method by which we publish the most recent image
# Specifically, this is used by the final image augmentation
# node as the models receive their image via the service.
self.pub_image = rospy.Publisher(
'image_server/image',
Image,
queue_size=1
)
rospy.loginfo("[IMG] Image Republisher Online")
# This publishes the status of the images delivered to the
# Model Server. It announces to all nodes that new images are
# available at the model server and they may now make a new
# request.
self.pub_status = rospy.Publisher(
'image_server/status',
Bool,
queue_size=1
)
rospy.loginfo("[IMG] Image Server Status Online")
def setup_model_server_proxy(self):
# This is the method API by which the image server requests
# that the model server accept its image. The request message
# is the image, and the reply is a bool indicating success.
self.image_push_rqst = rospy.ServiceProxy(
'model_server/image_fetching_service',
ImageRequest
)
def image_callback(self, msg):
# This callback function saves the image message received
# from the camera to self.image
self.image = msg
rospy.logdebug(f"[{self.name.abv}] Image Received.")
def image_rqst_callback(self, msg):
# If any message is received from a modeling node indicating a
# new image is required, this callback function sets the request
# ticket flag to True. This ensures that we only deliver images
# to the modeling server which will be used by the models.
node_status = msg.data
if node_status:
self.ticket.request = True
def action_loop(self):
self.ticket.request = True
if self.ticket.request:
self.timer.lap("Negotiate with Model Server")
# If request is true, then nodes are requesting images. If
# response is false, then we only need to republish the
# new image.
try:
rospy.loginfo("[IMG] Image Fetching Service Requested.")
response = self.image_push_rqst(self.image)
except:
rospy.logerr("[IMG] Could not send image to Model Server")
# If we couldn't deliver the image to the model server
# then we need to keep trying
return
if response.status.data:
# If True, the model server now has the newest image.
# Note that currently this should always be True,
# however that will not be the case as we offer
# different sized images
try:
# We tell all of the listening nodes they can now
# use the Model Server.
self.ticket.state = True
#we clear our request queue
self.ticket.request = False
rospy.loginfo("[IMG] Images Delivered to Model Server")
except:
rospy.logerr("[IMG] Could not broadcast image available")
# If we could not change status, we should still
# return the new image for nodes that are not
# dependent on the model server
return
self.timer.time(
"Negotiate with Model Server",
name = "MOD"
)
else:
rospy.loginfo("[IMG] Model Server returned False")
# If the ticket requests are set to false then the
# model server doesn't need a new image yet, however
# the other nodes which are not dependent on the model
# server do, so we will publish that image.
self.ticket.state = False
return
def loop(self):
while not rospy.is_shutdown():
print(self.img_s_parameters.rate)
print(self.img_s_parameters)
rate = rospy.Rate(self.img_s_parameters.rate)
# Update any of our parameters from the parameter server
if self.img_s_parameters.updates == True:
self.img_s_parameters.update()
self.timer = ProcessTimer(abv=self.name.abv)
# See if we need to service the Model Server
self.action_loop()
# Publish the current state of the image at the Model Server.
self.pub_status.publish(self.ticket.state)
# Publish the last image we received regardless.
self.pub_image.publish(self.image)
rospy.logdebug("[IMG] Image Published")
self.timer.time("total_runtime")
rate.sleep()
