Query handling in RoboKudo¶
In the previous tutorials, we have used RoboKudo to continuously analyze the incoming sensor data and visualize the results.
When integrating the system with a robot, often you are triggering the perception process during specific times in your high-level program when needed.
And additionally, you might supply additional information what to look for, like for example 'a red cup on a table'.
This is why RoboKudo offers a Query Interface, which allows you to formulate such queries to state the required perception task(like the 'red cup on table' in the example above).
In this tutorial, we will look at the central components to work with this interface.
Note
The Python pipeline examples target the current ROS 2 codebase. ROS 1 tabs are retained as legacy instructions for a ROS 1 checkout.
General idea¶
To handle queries, you need the following three key components:
Query retrieval in the pipeline: Receive the query, annotate it into the CAS and trigger the pipeline execution.
Interpret the query in your Annotators: Make your Annotators react to the query that is currently being processed.
Generate a query answer / result: Generate the result that should be send back to the caller.
Query retrieval in the pipeline¶
Import QueryAnnotator from robokudo.annotators.query and add QueryAnnotator() immediately after pipeline_init() in your pipeline.
The ROS 2 entry point starts the query action server; QueryAnnotator connects the pipeline to it and creates a server if one is not already registered. The server offers the robokudo_msgs/action/Query action at /robokudo/query. Its goal contains an obj field holding a robokudo_msgs/msg/ObjectDesignator, with attributes describing the requested object. These ROS interfaces are supplied by the separate cram_ros2_packages repository installed with your ROS workspace.
When the QueryAnnotator is visited in the Behavior Tree traversal, it will check if a new Query has been received. If no new Query is received, the Annotator will return Running.
Otherwise, it will annotate the Query into the CAS and return Success to allow the Behavior Tree to proceed to the actual perception pipeline to analyze the sensor data.
Interpret the query in your Annotators¶
After receiving the query, you will want to inspect the Query in your Annotators to check for the requested attributes. Access to it is provided by a predefined view in the CAS. The following code shows an example how to access it and react to it:
query = self.get_cas().query
if query is not None and query.obj.type == "Cup":
self.rk_logger.info("Query requests a cup; select a cup detector here")
This code belongs inside an Annotator method. Reading the query does not automatically select a detector or filter the results; your pipeline must implement that behavior.
Generate a query answer / result¶
At the end, you want to send back a result for the received query.
This is done by placing your query answer onto the Blackboard of the Behavior Tree.
The ActionServer in the QueryAnnotator will then pick up that answer and send the result to the caller of the Query.
In the following, you can see an example taken from an Annotator:
# .... your imports ...
from robokudo_msgs.msg import ObjectDesignator
from robokudo_msgs.action import Query
from robokudo.utils.query import QueryHandler
# The result of a Query is typically a list of ObjectDesignator instances
query_result = Query.Result()
object_designator = ObjectDesignator()
object_designator.type = 'Cup'
object_designator.color.append('red')
query_result.res.append(object_designator)
QueryHandler.send_answer(query_result)
# .... your imports ...
from robokudo_msgs.msg import QueryResult, ObjectDesignator
from robokudo.cas import CASViews
from robokudo.identifier import BBIdentifier
from py_trees.blackboard import Blackboard
# The result of a Query is typically a list of ObjectDesignator instances
query_result = QueryResult()
object_designator = ObjectDesignator()
object_designator.type = 'Cup'
object_designator.color.append('red')
query_result.res.append(object_designator)
blackboard = Blackboard()
blackboard.set(BBIdentifier.QUERY_ANSWER, query_result)
# Clear the query after we've answered it to indicate the the query is processed
self.get_cas().set(CASViews.QUERY, None)
Typically, such an Annotator would be at the end of the pipeline in your Analysis Engine, to conclude the perception pipeline and generate the final result based on the partial results in the CAS.
The ROS 2 example above only illustrates the message format: it returns a fixed cup description and does not detect a cup. For actual perception results, the built-in GenerateQueryResult Annotator converts supported annotations on the CAS’s object hypotheses into object designators. For example, use this ROS 2 analysis engine to run the demo segmentation on demand and return the detected objects with their colors:
from robokudo.analysis_engine import AnalysisEngineInterface
from robokudo.annotators.cluster_color import ClusterColorAnnotator
from robokudo.annotators.collection_reader import CollectionReaderAnnotator
from robokudo.annotators.image_preprocessor import ImagePreprocessorAnnotator
from robokudo.annotators.plane import PlaneAnnotator
from robokudo.annotators.pointcloud_cluster_extractor import PointCloudClusterExtractor
from robokudo.annotators.pointcloud_crop import PointcloudCropAnnotator
from robokudo.annotators.query import GenerateQueryResult, QueryAnnotator
from robokudo.descriptors.factories.cr_descriptor_factory import (
CollectionReaderDescriptorFactory,
)
from robokudo.idioms import pipeline_init
from robokudo.pipeline import Pipeline
class AnalysisEngine(AnalysisEngineInterface):
def name(self) -> str:
return "my_demo"
def implementation(self) -> Pipeline:
kinect_config = CollectionReaderDescriptorFactory.create_descriptor("kinect_wo_tf")
seq = Pipeline("QueryPipeline")
seq.add_children(
[
pipeline_init(),
QueryAnnotator(),
CollectionReaderAnnotator(descriptor=kinect_config),
ImagePreprocessorAnnotator("ImagePreprocessor"),
PointcloudCropAnnotator(),
PlaneAnnotator(),
PointCloudClusterExtractor(),
ClusterColorAnnotator(),
GenerateQueryResult(),
]
)
return seq
Save this as your package’s my_demo.py, rebuild and source your workspace if needed, and start it with ros2 run robokudo_ros main _ae=my_demo _ros_pkg=rk_tutorial. Keep the test bag playing as in the first tutorial. pipeline_init() waits for the action server to finish the previous goal before the next pipeline iteration.
By default, GenerateQueryResult returns all object hypotheses. To filter the answer by requested attributes, create a GenerateQueryResult.Descriptor(), set descriptor.parameters.filter_by_query = True, and pass it as GenerateQueryResult(descriptor=descriptor). Matching requires the corresponding annotations: this example computes colors but does not classify objects as cups.
Sending a query to RoboKudo¶
After starting RoboKudo with a QueryAnnotator in your analysis engine, you will get a new ROS action server at /robokudo/query.
You can either write an action client to send a request to this action server as described here or use the command line interface:
# Request a perception cycle without restricting the object attributes.
ros2 action send_goal /robokudo/query robokudo_msgs/action/Query '{obj: {}}' --feedback
Tip
Writing queries directly on the cli can be a bit cumbersome, you can also use a json file to store the query and use:
ros2 action send_goal /robokudo/query robokudo_msgs/action/Query "$(cat /path/to/your/query.json)" --feedback
Note
The ROS 1 axclient.py command below does not apply to ROS 2. The CLI example above can be used to test the ROS 2 action server.
You can either write an action client to send a request to this action server or use the axclient.py ROS node from the actionlib_tools package for a graphical user interface.
In ROS Noetic, you can install ros-noetic-actionlib-tools to get it.
Note
There is a tutorial on axclient.py usage here. Please note though, that the axclient.py node is now in the actionlib_tools package. This is currently wrong in the linked tutorial. The call should look like this:
rosrun actionlib_tools axclient.py /robokudo/query
Indicating a failure¶
If one of your Annotators encounters an error and cannot generate an answer, you can abort the action. The current ROS 2 implementation logs the detailed error on the RoboKudo side and returns a generic abort result to the caller.
We need two components for this: A decorator on the individual Annotator that should be able to return an error and an exception for the actual failure.
Many built-in Annotators of RoboKudo already have this Decorator.
If you are creating your own Annotator, make sure that its update method is annotated with the @robokudo.utils.error_handling.catch_and_raise_to_blackboard decorator.
Note
When your Annotator is extending/inheriting from the ThreadedAnnotator class, annotate the decorator on your compute() method instead.
After adding the decorator, you can indicate a failure by raising an exception. The decorator stores the exception on the Blackboard and returns Status.FAILURE. The current ROS 2 action server logs the exception and aborts the goal with text_result set to "Aborted"; it does not copy the exception text into the result. Returning Status.FAILURE alone does not store an exception for the action server.
Below, you can find a minimal example:
from py_trees.common import Status
from robokudo.utils.error_handling import catch_and_raise_to_blackboard
@catch_and_raise_to_blackboard
def update(self) -> Status:
color = self.get_cas().color_image
if color is None:
raise RuntimeError("No color image available")
# Process the color image here.
return Status.SUCCESS
Sending feedback during running perception queries¶
It is also possible to provide (ROS Action) feedback during the analysis of a query. You can simply add your feedback after getting the corresponding queue from the Blackboard:
from robokudo.utils.query import QueryHandler
QueryHandler.send_feedback_str("I'm still computing...")
feedback_queue: queue.Queue = blackboard.get(BBIdentifier.QUERY_FEEDBACK)
feedback_queue.put(f"I'm still computing...")