diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp index f2e87cfb..e6506abe 100644 --- a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -48,7 +48,7 @@ namespace fuse_models::parameters { /** - * @brief Defines the set of parameters required by the Odometry3D class + * @brief Defines the set of parameters required by the TransformSensor class */ struct TransformSensorParams : public ParameterBase { diff --git a/fuse_models/include/fuse_models/transform_sensor.hpp b/fuse_models/include/fuse_models/transform_sensor.hpp index 14c9e357..fa8523dc 100644 --- a/fuse_models/include/fuse_models/transform_sensor.hpp +++ b/fuse_models/include/fuse_models/transform_sensor.hpp @@ -56,20 +56,19 @@ namespace fuse_models { /** - * @brief An adapter-type sensor that produces pose constraints from AprilTag detections + * @brief An adapter-type sensor that produces pose constraints from published transforms * * This sensor subscribes to a MessageType topic and creates orientation and pose variables and constraints. - * - * This sensor really just separates out the orientation, angular velocity, and linear acceleration - * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D - * classes. + * This sensor can be used for AprilTags or any pose for which the transform to the desired state estimation frame is + * known. For an example, try `ros2 launch fuse_tutorials fuse_apriltag_tutorial.launch.py` and see its relevant files. * * Parameters: * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to * publish * - device_name (string) Used to generate the device/robot ID if the device_id is not provided - * - queue_size (int, default: 10) The subscriber queue size for the pose messages - * - topic (string) The topic to which to subscribe for the pose messages + * - queue_size (int, default: 10) The subscriber queue size for the transform messages + * - topic (string) The topic to which to subscribe for the transform messages + * - target_frame (string) the state estimation frame to transform tfs to * * Subscribes: * - \p topic (MessageType) IMU data at a given timestep @@ -103,7 +102,7 @@ class TransformSensor : public fuse_core::AsyncSensorModel const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** - * @brief Callback for pose messages + * @brief Callback for tf messages * @param[in] msg - The IMU message to process */ void process(const MessageType& msg); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 630ce3d2..f8816a29 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -515,12 +515,17 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ // And purge out old transactions // - Either before or exactly at the start time // - Or with a minimum time before the minimum time of this ignition sensor transaction + // + // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we + // can use std::as_const: + // https://en.cppreference.com/w/cpp/utility/as_const pending_transactions_.erase( std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, &min_time = std::as_const(start_time)](const auto& transaction) { + [&sensor_name, max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) return transaction.sensor_name != sensor_name && (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), + }), // NOLINT(whitespace/braces) pending_transactions_.end()); } else