Class RangeSensorModel

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public fuse_core::AsyncSensorModel

Class Documentation

class RangeSensorModel : public fuse_core::AsyncSensorModel

Implements a range-only sensor model that generates constraints between the robot and a beacon.

The main purpose for this sensor model is to demonstrate how to write your own sensor model classes.

For the purposes of this tutorial, let’s imagine that you have developed a new robotic sensor that is capable of measuring the distance to some sort of beacon, but does not provide any information about the bearing/heading to that beacon. None of the fuse packages provide such a sensor model, so you need to develop one yourself. Because I don’t want to create a brand new message type for this tutorial, the driver for this new sensor will be publishing sensor_msgs::msg::PointCloud2 messages with the following fields defined:

  • ”id”, uint32, count 1, offset 0, The unique ID associated with that beacon

  • ”range”, float64, count 1, offset 4, The range, in meters, between the robot and the beacon

  • ”sigma”, float64, count 1, offset 12, The standard deviation of the range measurement, in meters

The “sensor model” class provides an interface to ROS, allowing sensor messages to be received. The sensor model class also acts as a “factory” (in a programming sense) that creates new sensor constraints for each received sensor measurement, and forward those constraints the fuse optimizer. The optimizer is where the constraints from all configured sensors are combined, and the best possible value for each state variable is determined.

Each fuse SensorModel is implemented as a plugin, which is loaded by the optimizer at runtime. This allows new sensor models to be implemented outside of the main fuse package, such as in this fuse_tutorials package. The fuse SensorModel base class defines a few basic methods for communicating between the derived SensorModel and the optimizer.

  • initialize() This is called by the optimizer after construction. This is a common pattern used by plugins. This is often when the Parameter Server is queried for configuration data.

  • start() Tells the sensor model to start producing constraints. This is commonly where fuse sensor models first subscribe to their sensor data topics.

  • stop() Tells the sensor model to stop producing constraints. fuse sensor models typically unsubscribe from topics here.

  • graphCallback() The optimizer provides the sensor model with the latest set of optimized states. For simple sensor models, this is likely not needed. But something maintaining a database of visual landmarks or similar may need access to the current set of landmarks.

  • TransactionCallback This is a little different than the other interfaces. This is a callback provided to the SensorModel plugin. That callback is executed by the plugin whenever new constraints are ready to be sent to the optimizer.

An issue with the SensorModel base class is a rather complex threading model, where TransactionCallbacks can be executed at any time by any of the sensor model plugins. To make derived sensor models easier to implement, the AsyncSensorModel base class is provided, which hides most of the thread synchronization details. The AsyncSensorModel class provides its own callback queue and spinner, making it act much like a typical, single-threaded ROS node. All of the original base SensorModel methods are wrapped. Instead, slightly modified versions are provided, which are executed within the AsyncSensorModel’s spinner thread.

Additionally, the AsyncSensorModel base class provides a public and private node handle that are pre-configured for use with the spinner’s thread and callback queue. These should be used instead of creating your own, in much the same way that nodelets provide methods for accessing properly configured node handles.

All of the sensor models provided by the fuse_models package use the AsyncSensorModel base class, and it is the recommended way to start developing new sensor models.

Public Functions

inline RangeSensorModel()

Default constructor.

A default constructor is required by pluginlib. The real initialization of the sensor model will occur in the onInit() method. This will be called immediately after construction by the optimizer node. We do, however, specify the number of threads to use to spin the callback queue. Generally this will be 1, unless you have a good reason to use a multi-threaded spinner.

void initialize(fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces, const std::string &name, fuse_core::TransactionCallback transaction_callback) override

Shadowing extension to the AsyncSensorModel::initialize call.

void priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 &msg)

Receives the set of known beacon positions.

We need a method to initialize the position of each beacon variable. In this tutorial we are assuming a database exists of previously measured but noisy beacon positions. That database will be published on a latched topic. This sensor model will subscribe to that topic, and use that prior information to initialize and properly constrain the beacon positions within the optimizer. No range measurements will be processed until this database has been received.

Parameters:

msg – [in] - Message containing the database of known but noisy beacon positions.

void rangesCallback(const sensor_msgs::msg::PointCloud2 &msg)

Callback for range measurement messages.

We will process all of the detected beacons in the input message, generate one or more RangeConstraint objects, and send all of the constraints to the optimizer at once packaged in a Transaction object.

Parameters:

msg – [in] - The range message to process

Protected Functions

virtual void onInit() override

Perform any required initialization for the sensor model.

This could include things like reading from the parameter server or subscribing to topics. The class’s node handles will be properly initialized before AsyncSensorModel::onInit() is called. Spinning of the callback queue will not begin until after the call to AsyncSensorModel::onInit() completes.

virtual void onStart() override

Subscribe to the input topic to start sending transactions to the optimizer.

virtual void onStop() override

Unsubscribe from the input topic to stop sending transactions to the optimizer.

Protected Attributes

fuse_core::node_interfaces::NodeInterfaces<fuse_core::node_interfaces::Base, fuse_core::node_interfaces::Logging, fuse_core::node_interfaces::Topics, fuse_core::node_interfaces::Waitables> interfaces_

Shadows AsyncSensorModel interfaces_.

rclcpp::Logger logger_

The sensor model’s logger.

std::unordered_map<unsigned int, Beacon> beacon_db_

The estimated position of each beacon.

ROS subscription for the database of prior beacon positions

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr beacon_sub_
bool initialized_ = {false}

Flag indicating the initial beacon positions have been processed.

ROS subscription for the range sensor measurements

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_
struct Beacon

Public Members

double x
double y
double sigma