diff --git a/fuse/package.xml b/fuse/package.xml
index ed8b560a8..fc7a7c755 100644
--- a/fuse/package.xml
+++ b/fuse/package.xml
@@ -12,6 +12,7 @@
catkin
fuse_core
+ fuse_variables
diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt
new file mode 100644
index 000000000..1dea655da
--- /dev/null
+++ b/fuse_variables/CMakeLists.txt
@@ -0,0 +1,179 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(fuse_variables)
+
+find_package(catkin REQUIRED COMPONENTS
+ fuse_core
+ roscpp
+)
+
+catkin_package(
+ INCLUDE_DIRS
+ include
+ LIBRARIES
+ ${PROJECT_NAME}
+ CATKIN_DEPENDS
+ fuse_core
+ roscpp
+)
+
+###########
+## Build ##
+###########
+
+set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++14;-Wall;-Werror")
+
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## fuse_variables library
+add_library(${PROJECT_NAME}
+ src/acceleration_angular_2d_stamped.cpp
+ src/acceleration_linear_2d_stamped.cpp
+ src/orientation_2d_stamped.cpp
+ src/position_2d_stamped.cpp
+ src/position_3d_stamped.cpp
+ src/velocity_angular_2d_stamped.cpp
+ src/velocity_linear_2d_stamped.cpp
+)
+add_dependencies(${PROJECT_NAME}
+ ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(
+ TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/fuse_locus_variables/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(Ceres REQUIRED)
+ find_package(roslint REQUIRED)
+ find_package(rostest REQUIRED)
+ include_directories(${CERES_INCLUDE_DIRS})
+
+ # Lint tests
+ set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
+ roslint_cpp()
+ roslint_add_test()
+
+ # Acceleration Angular 2D Stamped Tests
+ catkin_add_gtest(test_acceleration_angular_2d_stamped
+ test/test_acceleration_angular_2d_stamped.cpp
+ )
+ add_dependencies(test_acceleration_angular_2d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_acceleration_angular_2d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+
+ # Acceleration Linear 2D Stamped Tests
+ catkin_add_gtest(test_acceleration_linear_2d_stamped
+ test/test_acceleration_linear_2d_stamped.cpp
+ )
+ add_dependencies(test_acceleration_linear_2d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_acceleration_linear_2d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+
+ # Fixed Size Variable Tests
+ catkin_add_gtest(test_fixed_size_variable
+ test/test_fixed_size_variable.cpp
+ )
+ add_dependencies(test_fixed_size_variable
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_fixed_size_variable
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ )
+
+ # Orientation 2D Stamped Tests
+ catkin_add_gtest(test_orientation_2d_stamped
+ test/test_orientation_2d_stamped.cpp
+ )
+ add_dependencies(test_orientation_2d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_orientation_2d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+
+ # Position 2D Stamped Tests
+ catkin_add_gtest(test_position_2d_stamped
+ test/test_position_2d_stamped.cpp
+ )
+ add_dependencies(test_position_2d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_position_2d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+
+ # Position 3D stamped tests
+ catkin_add_gtest(test_position_3d_stamped
+ test/test_position_3d_stamped.cpp
+ )
+ add_dependencies(test_position_3d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_position_3d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+
+ # Velocity Angular 2D Stamped Tests
+ catkin_add_gtest(test_velocity_angular_2d_stamped
+ test/test_velocity_angular_2d_stamped.cpp
+ )
+ add_dependencies(test_velocity_angular_2d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_velocity_angular_2d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+
+ # Velocity Linear 2D Stamped Tests
+ catkin_add_gtest(test_velocity_linear_2d_stamped
+ test/test_velocity_linear_2d_stamped.cpp
+ )
+ add_dependencies(test_velocity_linear_2d_stamped
+ ${catkin_EXPORTED_TARGETS}
+ )
+ target_link_libraries(test_velocity_linear_2d_stamped
+ ${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ ${CERES_LIBRARIES}
+ )
+endif()
diff --git a/fuse_variables/LICENSE b/fuse_variables/LICENSE
new file mode 100644
index 000000000..3ff0a6145
--- /dev/null
+++ b/fuse_variables/LICENSE
@@ -0,0 +1,31 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2018, Locus Robotics
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above
+ copyright notice, this list of conditions and the following
+ disclaimer in the documentation and/or other materials provided
+ with the distribution.
+ * Neither the name of the copyright holder nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/fuse_variables/README.md b/fuse_variables/README.md
new file mode 100644
index 000000000..753c7d28d
--- /dev/null
+++ b/fuse_variables/README.md
@@ -0,0 +1,3 @@
+# fuse_variables
+This package provides a set of commonly used variable types, such as 2D and 3D positions, orientations, velocities,
+and accelerations.
diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h
new file mode 100644
index 000000000..836581304
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h
@@ -0,0 +1,125 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_ACCELERATION_ANGULAR_2D_STAMPED_H
+#define FUSE_VARIABLES_ACCELERATION_ANGULAR_2D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 2D angular acceleration at a specific time, with a specific piece of hardware.
+ *
+ * This is commonly used to represent a robot's acceleration. The UUID of this class is constant after construction.
+ * As such, the timestamp and hardware id cannot be modified. The value of the acceleration can be modified.
+ */
+class AccelerationAngular2DStamped final : public FixedSizeVariable<1>
+{
+public:
+ SMART_PTR_DEFINITIONS(AccelerationAngular2DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ AccelerationAngular2DStamped();
+
+ /**
+ * @brief Construct a 2D acceleration at a specific point in time.
+ *
+ * @param[in] stamp The timestamp attached to this velocity.
+ * @param[in] hardware_id An optional hardware id, for use when variables originate from multiple robots or devices
+ */
+ explicit AccelerationAngular2DStamped(
+ const ros::Time& stamp,
+ const fuse_core::UUID& hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the angular acceleration.
+ */
+ double& atheta() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the angular acceleration.
+ */
+ const double& atheta() const { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the associated hardware ID.
+ */
+ const fuse_core::UUID& hardwareId() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param[out] stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The hardware UUID associated with this variable instance
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_ACCELERATION_ANGULAR_2D_STAMPED_H
diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h
new file mode 100644
index 000000000..3ce6a41d1
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h
@@ -0,0 +1,136 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_ACCELERATION_LINEAR_2D_STAMPED_H
+#define FUSE_VARIABLES_ACCELERATION_LINEAR_2D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 2D linear acceleration (ax, ay) at a specific time, with a specific piece of hardware.
+ *
+ * This is commonly used to represent a robot's acceleration. The UUID of this class is static after construction.
+ * As such, the timestamp and hardware id cannot be modified (with the exception of the deserializeMessage() function).
+ * The value of the acceleration can be modified.
+ */
+class AccelerationLinear2DStamped final : public FixedSizeVariable<2>
+{
+public:
+ SMART_PTR_DEFINITIONS(AccelerationLinear2DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ AccelerationLinear2DStamped();
+
+ /**
+ * @brief Construct a 2D acceleration at a specific point in time.
+ *
+ * @param[in] stamp The timestamp attached to this velocity.
+ * @param[in] hardware_id An optional hardware id, for use when variables originate from multiple robots or devices
+ */
+ explicit AccelerationLinear2DStamped(
+ const ros::Time& stamp,
+ const fuse_core::UUID& hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the X-axis linear acceleration.
+ */
+ double& ax() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the X-axis linear acceleration.
+ */
+ const double& ax() const { return data_[0]; }
+
+ /**
+ * @brief Read-write access to the Y-axis linear acceleration.
+ */
+ double& ay() { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the Y-axis linear acceleration.
+ */
+ const double& ay() const { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the associated hardware ID.
+ */
+ const fuse_core::UUID& hardwareId() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param[out] stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The hardware UUID associated with this variable instance
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_ACCELERATION_LINEAR_2D_STAMPED_H
diff --git a/fuse_variables/include/fuse_variables/fixed_size_variable.h b/fuse_variables/include/fuse_variables/fixed_size_variable.h
new file mode 100644
index 000000000..7e3e4ed6d
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/fixed_size_variable.h
@@ -0,0 +1,113 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_FIXED_SIZE_VARIABLE_H
+#define FUSE_VARIABLES_FIXED_SIZE_VARIABLE_H
+
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief A Variable base class for fixed-sized variables
+ *
+ * The FixedSizeVariable class implements a statically sized array to hold the scalar values. The size of the variable
+ * is provided as the template argument \p N when creating a derived class. The FixedSizeVariable class implements the
+ * Variable::data() accessor methods, and provides access to the scalar values as a std::array. This allows easier
+ * manipulation in C++ (iterators, range-based for loops, etc.). The FixedSizeVariable class is designed for variables
+ * where the size of the state vector is known at compile time...which should be almost all variable types. The
+ * dimension of typical variable types (points, poses, calibration parameters) are all known at design/compile time.
+ */
+template
+class FixedSizeVariable : public fuse_core::Variable
+{
+public:
+ SMART_PTR_ALIASES_ONLY(FixedSizeVariable);
+
+ /**
+ * @brief A static version of the variable size
+ */
+ static const size_t SIZE = N;
+
+ /**
+ * @brief Constructor
+ */
+ FixedSizeVariable() :
+ Variable(),
+ data_{} // zero-initialize the data array
+ {}
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~FixedSizeVariable() = default;
+
+ /**
+ * @brief Returns the number of elements of this variable.
+ *
+ * The number of scalar values contained by this variable type is defined by the class template parameter \p N.
+ */
+ size_t size() const override { return N; }
+
+ /**
+ * @brief Read-only access to the variable data
+ */
+ const double* data() const override { return data_.data(); }
+
+ /**
+ * @brief Read-write access to the variable data
+ */
+ double* data() override { return data_.data(); }
+
+ /**
+ * @brief Read-only access to the variable data as a std::array
+ */
+ const std::array& array() const { return data_; }
+
+ /**
+ * @brief Read-write access to the variable data as a std::array
+ */
+ std::array& array() { return data_; }
+
+protected:
+ std::array data_; //!< Fixed-sized, contiguous memory for holding the variable data members
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_FIXED_SIZE_VARIABLE_H
diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.h b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h
new file mode 100644
index 000000000..5cfa8d16a
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h
@@ -0,0 +1,162 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_ORIENTATION_2D_STAMPED_H
+#define FUSE_VARIABLES_ORIENTATION_2D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 2D orientation (theta) at a specific time, with a specific piece of hardware.
+ *
+ * This is commonly used to represent a robot's orientation within a map. The UUID of this class is static after
+ * construction. As such, the timestamp and hardware id cannot be modified (with the exception of the
+ * deserializeMessage() function). The value of the orientation can be modified.
+ */
+class Orientation2DStamped final : public FixedSizeVariable<1>
+{
+public:
+ SMART_PTR_DEFINITIONS(Orientation2DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ Orientation2DStamped();
+
+ /**
+ * @brief Construct a 2D orientation at a specific point in time.
+ *
+ * @param[in] stamp The timestamp attached to this orientation.
+ * @param[in] hardware_id An optional hardware id, for use when variables originate from multiple robots or devices
+ *
+ */
+ explicit Orientation2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the heading angle.
+ */
+ double& theta() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the heading angle.
+ */
+ const double& theta() const { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the associated hardware ID.
+ */
+ const fuse_core::UUID& hardwareId() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param[out] stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+ /**
+ * @brief Create a new Ceres local parameterization object to apply to updates of this variable
+ *
+ * A 2D rotation has a nonlinearity when the angle wraps around from -PI to PI. This is handled by a custom
+ * local parameterization to ensure smooth derivatives.
+ *
+ * @return A base pointer to an instance of a derived LocalParameterization
+ */
+ ceres::LocalParameterization* localParameterization() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The hardware UUID associated with this variable instance
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+
+ /**
+ * @brief Functor that computes an incremental update to a 2D orientation. This handles the 2*Pi rollover.
+ *
+ * This function is designed for use with Google's Ceres optimization engine. The Ceres variation of std::floor is
+ * used, which has been specialized for Jet datatypes.
+ */
+ struct Orientation2DPlus
+ {
+ template
+ bool operator()(const T* x, const T* delta, T* x_plus_delta) const
+ {
+ // Define some necessary variations of PI with the correct type (double or Jet)
+ static const T PI = T(M_PI);
+ static const T TWO_PI = T(2 * M_PI);
+
+ // Compute the angle increment as a linear update
+ x_plus_delta[0] = x[0] + delta[0];
+ // Then handle the 2*Pi roll-over
+ // Use ceres::floor because it is specialized for double and Jet types.
+ x_plus_delta[0] -= TWO_PI * ceres::floor((x_plus_delta[0] + PI) / TWO_PI);
+ return true;
+ }
+ };
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_ORIENTATION_2D_STAMPED_H
diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.h b/fuse_variables/include/fuse_variables/position_2d_stamped.h
new file mode 100644
index 000000000..63a4c994c
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/position_2d_stamped.h
@@ -0,0 +1,135 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_POSITION_2D_STAMPED_H
+#define FUSE_VARIABLES_POSITION_2D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 2D position (x, y) at a specific time, with a specific piece of hardware.
+ *
+ * This is commonly used to represent a robot's position within a map. The UUID of this class is static after
+ * construction. As such, the timestamp and hardware id cannot be modified (with the exception of the
+ * deserializeMessage() function). The value of the position can be modified.
+ */
+class Position2DStamped final : public FixedSizeVariable<2>
+{
+public:
+ SMART_PTR_DEFINITIONS(Position2DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ Position2DStamped();
+
+ /**
+ * @brief Construct a 2D position at a specific point in time.
+ *
+ * @param[in] stamp The timestamp attached to this position.
+ * @param[in] hardware_id An optional hardware id, for use when variables originate from multiple robots or devices
+ *
+ */
+ explicit Position2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the X-axis position.
+ */
+ double& x() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the X-axis position.
+ */
+ const double& x() const { return data_[0]; }
+
+ /**
+ * @brief Read-write access to the Y-axis position.
+ */
+ double& y() { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the Y-axis position.
+ */
+ const double& y() const { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the associated hardware ID.
+ */
+ const fuse_core::UUID& hardwareId() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param[out] stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The hardware UUID associated with this variable instance
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_POSITION_2D_STAMPED_H
diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.h b/fuse_variables/include/fuse_variables/position_3d_stamped.h
new file mode 100644
index 000000000..195818022
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/position_3d_stamped.h
@@ -0,0 +1,144 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_POSITION_3D_STAMPED_H
+#define FUSE_VARIABLES_POSITION_3D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 3D position (x, y, z) at a specific time and for a specific piece of hardware
+ * (e.g., robot)
+ *
+ * This is commonly used to represent a robot position in single or multi-robot systems. The UUID of this class is
+ * static after construction. As such, the timestamp and hardware ID cannot be modified (with the exception of the
+ * deserializeMessage() function). The value of the position can be modified.
+ */
+class Position3DStamped : public FixedSizeVariable<3>
+{
+public:
+ SMART_PTR_DEFINITIONS(Position3DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ Position3DStamped();
+
+ /**
+ * @brief Construct a 3D position at a specific point in time.
+ *
+ * @param[IN] stamp The timestamp attached to this popositionse.
+ */
+ explicit Position3DStamped(const ros::Time& stamp, const fuse_core::UUID &hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the X-axis position.
+ */
+ double& x() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the X-axis position.
+ */
+ const double& x() const { return data_[0]; }
+
+ /**
+ * @brief Read-write access to the Y-axis position.
+ */
+ double& y() { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the Y-axis position.
+ */
+ const double& y() const { return data_[1]; }
+
+ /**
+ * @brief Read-write access to the Z-axis position.
+ */
+ double& z() { return data_[2]; }
+
+ /**
+ * @brief Read-only access to the Z-axis position.
+ */
+ const double& z() const { return data_[2]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the unique ID of the hardware device (e.g., robot) for which this variable is measured
+ */
+ const fuse_core::UUID hardware_id() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The UUID corresponding to the hardware device for which this variable is measured
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_POSITION_3D_STAMPED_H
diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h
new file mode 100644
index 000000000..e6ae0b5bb
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h
@@ -0,0 +1,124 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_VELOCITY_ANGULAR_2D_STAMPED_H
+#define FUSE_VARIABLES_VELOCITY_ANGULAR_2D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 2D angular velocity (vtheta) at a specific time, with a specific piece of hardware.
+ *
+ * This is commonly used to represent a robot's velocity. The UUID of this class is static after construction.
+ * As such, the timestamp and hardware id cannot be modified (with the exception of the deserializeMessage() function).
+ * The value of the velocity can be modified.
+ */
+class VelocityAngular2DStamped final : public FixedSizeVariable<1>
+{
+public:
+ SMART_PTR_DEFINITIONS(VelocityAngular2DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ VelocityAngular2DStamped();
+
+ /**
+ * @brief Construct a 2D velocity at a specific point in time.
+ *
+ * @param[in] stamp The timestamp attached to this velocity.
+ * @param[in] hardware_id An optional hardware id, for use when variables originate from multiple robots or devices
+ */
+ explicit VelocityAngular2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the angular velocity.
+ */
+ double& vtheta() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the angular velocity.
+ */
+ const double& vtheta() const { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the associated hardware ID.
+ */
+ const fuse_core::UUID& hardwareId() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param[out] stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The hardware UUID associated with this variable instance
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_VELOCITY_ANGULAR_2D_STAMPED_H
diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h
new file mode 100644
index 000000000..f99c1cedd
--- /dev/null
+++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h
@@ -0,0 +1,135 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_VARIABLES_VELOCITY_LINEAR_2D_STAMPED_H
+#define FUSE_VARIABLES_VELOCITY_LINEAR_2D_STAMPED_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+/**
+ * @brief Variable representing a 2D linear velocity (vx, vy) at a specific time, with a specific piece of hardware.
+ *
+ * This is commonly used to represent a robot's velocity. The UUID of this class is static after construction.
+ * As such, the timestamp and hardware id cannot be modified (with the exception of the deserializeMessage() function).
+ * The value of the velocity can be modified.
+ */
+class VelocityLinear2DStamped : public FixedSizeVariable<2>
+{
+public:
+ SMART_PTR_DEFINITIONS(VelocityLinear2DStamped);
+
+ /**
+ * @brief Default constructor
+ *
+ * This is needed for the ROS plugin system and the deserializeMessage() method. It should not be used directly.
+ */
+ VelocityLinear2DStamped();
+
+ /**
+ * @brief Construct a 2D velocity at a specific point in time.
+ *
+ * @param[in] stamp The timestamp attached to this velocity.
+ * @param[in] hardware_id An optional hardware id, for use when variables originate from multiple robots or devices
+ *
+ */
+ explicit VelocityLinear2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id = fuse_core::uuid::NIL);
+
+ /**
+ * @brief Read-write access to the X-axis linear velocity.
+ */
+ double& vx() { return data_[0]; }
+
+ /**
+ * @brief Read-only access to the X-axis linear velocity.
+ */
+ const double& vx() const { return data_[0]; }
+
+ /**
+ * @brief Read-write access to the Y-axis linear velocity.
+ */
+ double& vy() { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the Y-axis linear velocity.
+ */
+ const double& vy() const { return data_[1]; }
+
+ /**
+ * @brief Read-only access to the associated timestamp.
+ */
+ const ros::Time& stamp() const { return stamp_; }
+
+ /**
+ * @brief Read-only access to the associated hardware ID.
+ */
+ const fuse_core::UUID& hardwareId() const { return hardware_id_; }
+
+ /**
+ * @brief Read-only access to the unique ID of this variable instance.
+ *
+ * All variables of this type with identical timestamps will return the same UUID.
+ */
+ fuse_core::UUID uuid() const override { return uuid_; }
+
+ /**
+ * @brief Print a human-readable description of the variable to the provided stream.
+ *
+ * @param[out] stream The stream to write to. Defaults to stdout.
+ */
+ void print(std::ostream& stream = std::cout) const override;
+
+ /**
+ * @brief Perform a deep copy of the Variable and return a unique pointer to the copy
+ *
+ * @return A unique pointer to a new instance of the most-derived Variable
+ */
+ fuse_core::Variable::UniquePtr clone() const override;
+
+protected:
+ fuse_core::UUID hardware_id_; //!< The hardware UUID associated with this variable instance
+ ros::Time stamp_; //!< The timestamp associated with this variable instance
+ fuse_core::UUID uuid_; //!< The UUID for this instance, computed during construction
+};
+
+} // namespace fuse_variables
+
+#endif // FUSE_VARIABLES_VELOCITY_LINEAR_2D_STAMPED_H
diff --git a/fuse_variables/package.xml b/fuse_variables/package.xml
new file mode 100644
index 000000000..c78c3596e
--- /dev/null
+++ b/fuse_variables/package.xml
@@ -0,0 +1,20 @@
+
+
+ fuse_variables
+ 0.3.1
+
+ The fuse_variables package provides a set of commonly used variable types, such as 2D and 3D positions,
+ orientations, velocities, and accelerations.
+
+
+ Stephen Williams
+ Stephen Williams
+ BSD
+
+ catkin
+ fuse_core
+ roscpp
+ ceres-solver
+ roslint
+ rostest
+
diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp
new file mode 100644
index 000000000..486301a2c
--- /dev/null
+++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp
@@ -0,0 +1,71 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+
+namespace fuse_variables
+{
+
+AccelerationAngular2DStamped::AccelerationAngular2DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+AccelerationAngular2DStamped::AccelerationAngular2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp, hardware_id))
+{
+}
+
+void AccelerationAngular2DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " hardware_id: " << hardwareId() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - atheta: " << atheta() << "\n";
+}
+
+fuse_core::Variable::UniquePtr AccelerationAngular2DStamped::clone() const
+{
+ return AccelerationAngular2DStamped::make_unique(*this);
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp
new file mode 100644
index 000000000..f8cae9c8d
--- /dev/null
+++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp
@@ -0,0 +1,72 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+
+namespace fuse_variables
+{
+
+AccelerationLinear2DStamped::AccelerationLinear2DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+AccelerationLinear2DStamped::AccelerationLinear2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp, hardware_id))
+{
+}
+
+void AccelerationLinear2DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " hardware_id: " << hardwareId() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - ax: " << ax() << "\n"
+ << " - ay: " << ay() << "\n";
+}
+
+fuse_core::Variable::UniquePtr AccelerationLinear2DStamped::clone() const
+{
+ return AccelerationLinear2DStamped::make_unique(*this);
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp
new file mode 100644
index 000000000..da22c7701
--- /dev/null
+++ b/fuse_variables/src/orientation_2d_stamped.cpp
@@ -0,0 +1,78 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+
+
+namespace fuse_variables
+{
+
+Orientation2DStamped::Orientation2DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+Orientation2DStamped::Orientation2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp, hardware_id))
+{
+}
+
+void Orientation2DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " hardware_id: " << hardwareId() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - theta: " << theta() << "\n";
+}
+
+fuse_core::Variable::UniquePtr Orientation2DStamped::clone() const
+{
+ return Orientation2DStamped::make_unique(*this);
+}
+
+ceres::LocalParameterization* Orientation2DStamped::localParameterization() const
+{
+ return new ceres::AutoDiffLocalParameterization();
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp
new file mode 100644
index 000000000..34fb2f809
--- /dev/null
+++ b/fuse_variables/src/position_2d_stamped.cpp
@@ -0,0 +1,72 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+
+namespace fuse_variables
+{
+
+Position2DStamped::Position2DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+Position2DStamped::Position2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp, hardware_id))
+{
+}
+
+void Position2DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " hardware_id: " << hardwareId() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - x: " << x() << "\n"
+ << " - y: " << y() << "\n";
+}
+
+fuse_core::Variable::UniquePtr Position2DStamped::clone() const
+{
+ return Position2DStamped::make_unique(*this);
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp
new file mode 100644
index 000000000..ac1906205
--- /dev/null
+++ b/fuse_variables/src/position_3d_stamped.cpp
@@ -0,0 +1,73 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+
+namespace fuse_variables
+{
+
+Position3DStamped::Position3DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+Position3DStamped::Position3DStamped(const ros::Time& stamp, const fuse_core::UUID &hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp_, hardware_id_))
+{
+}
+
+void Position3DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " hardware id: " << hardware_id() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - x: " << x() << "\n"
+ << " - y: " << y() << "\n"
+ << " - z: " << z() << "\n";
+}
+
+fuse_core::Variable::UniquePtr Position3DStamped::clone() const
+{
+ return Position3DStamped::make_unique(*this);
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp
new file mode 100644
index 000000000..4edbc09cd
--- /dev/null
+++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp
@@ -0,0 +1,71 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+
+namespace fuse_variables
+{
+
+VelocityAngular2DStamped::VelocityAngular2DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+VelocityAngular2DStamped::VelocityAngular2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp, hardware_id))
+{
+}
+
+void VelocityAngular2DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " hardware_id: " << hardwareId() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - vtheta: " << vtheta() << "\n";
+}
+
+fuse_core::Variable::UniquePtr VelocityAngular2DStamped::clone() const
+{
+ return VelocityAngular2DStamped::make_unique(*this);
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp
new file mode 100644
index 000000000..352fe736b
--- /dev/null
+++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp
@@ -0,0 +1,72 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+
+namespace fuse_variables
+{
+
+VelocityLinear2DStamped::VelocityLinear2DStamped() :
+ hardware_id_(fuse_core::uuid::NIL),
+ stamp_(0, 0),
+ uuid_(fuse_core::uuid::NIL)
+{
+}
+
+VelocityLinear2DStamped::VelocityLinear2DStamped(const ros::Time& stamp, const fuse_core::UUID& hardware_id) :
+ hardware_id_(hardware_id),
+ stamp_(stamp),
+ uuid_(fuse_core::uuid::generate(type(), stamp, hardware_id))
+{
+}
+
+void VelocityLinear2DStamped::print(std::ostream& stream) const
+{
+ stream << type() << ":\n"
+ << " uuid: " << uuid() << "\n"
+ << " stamp: " << stamp() << "\n"
+ << " hardware_id: " << hardwareId() << "\n"
+ << " size: " << size() << "\n"
+ << " data:\n"
+ << " - vx: " << vx() << "\n"
+ << " - vy: " << vy() << "\n";
+}
+
+fuse_core::Variable::UniquePtr VelocityLinear2DStamped::clone() const
+{
+ return VelocityLinear2DStamped::make_unique(*this);
+}
+
+} // namespace fuse_variables
diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp
new file mode 100644
index 000000000..f8dea96bf
--- /dev/null
+++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp
@@ -0,0 +1,130 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::AccelerationAngular2DStamped;
+
+
+TEST(AccelerationAngular2DStamped, Type)
+{
+ AccelerationAngular2DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::AccelerationAngular2DStamped", variable.type());
+}
+
+TEST(AccelerationAngular2DStamped, UUID)
+{
+ // Verify two accelerations at the same timestamp produce the same UUID
+ {
+ AccelerationAngular2DStamped variable1(ros::Time(12345678, 910111213));
+ AccelerationAngular2DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+
+ AccelerationAngular2DStamped variable3(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ AccelerationAngular2DStamped variable4(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ EXPECT_EQ(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two accelerations at different timestamps produce different UUIDs
+ {
+ AccelerationAngular2DStamped variable1(ros::Time(12345678, 910111213));
+ AccelerationAngular2DStamped variable2(ros::Time(12345678, 910111214));
+ AccelerationAngular2DStamped variable3(ros::Time(12345679, 910111213));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ EXPECT_NE(variable1.uuid(), variable3.uuid());
+ EXPECT_NE(variable2.uuid(), variable3.uuid());
+ }
+
+ // Verify two accelerations with different hardware IDs produce different UUIDs
+ {
+ AccelerationAngular2DStamped variable1(ros::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2"));
+ AccelerationAngular2DStamped variable2(ros::Time(12345678, 910111213), fuse_core::uuid::generate("bb8"));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(2.7);
+ return true;
+ }
+};
+
+TEST(AccelerationAngular2DStamped, Optimization)
+{
+ // Create a AccelerationAngular2DStamped
+ AccelerationAngular2DStamped acceleration(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000"));
+ acceleration.atheta() = 1.5;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ acceleration.data(),
+ acceleration.size(),
+ acceleration.localParameterization());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(acceleration.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(2.7, acceleration.atheta(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp
new file mode 100644
index 000000000..84bb12a34
--- /dev/null
+++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp
@@ -0,0 +1,133 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::AccelerationLinear2DStamped;
+
+
+TEST(AccelerationLinear2DStamped, Type)
+{
+ AccelerationLinear2DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::AccelerationLinear2DStamped", variable.type());
+}
+
+TEST(AccelerationLinear2DStamped, UUID)
+{
+ // Verify two accelerations at the same timestamp produce the same UUID
+ {
+ AccelerationLinear2DStamped variable1(ros::Time(12345678, 910111213));
+ AccelerationLinear2DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+
+ AccelerationLinear2DStamped variable3(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ AccelerationLinear2DStamped variable4(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ EXPECT_EQ(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two accelerations at different timestamps produce different UUIDs
+ {
+ AccelerationLinear2DStamped variable1(ros::Time(12345678, 910111213));
+ AccelerationLinear2DStamped variable2(ros::Time(12345678, 910111214));
+ AccelerationLinear2DStamped variable3(ros::Time(12345679, 910111213));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ EXPECT_NE(variable1.uuid(), variable3.uuid());
+ EXPECT_NE(variable2.uuid(), variable3.uuid());
+ }
+
+ // Verify two accelerations with different hardware IDs produce different UUIDs
+ {
+ AccelerationLinear2DStamped variable1(ros::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2"));
+ AccelerationLinear2DStamped variable2(ros::Time(12345678, 910111213), fuse_core::uuid::generate("bb8"));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(3.0);
+ residual[1] = x[1] + T(8.0);
+ return true;
+ }
+};
+
+TEST(AccelerationLinear2DStamped, Optimization)
+{
+ // Create a AccelerationLinear2DStamped
+ AccelerationLinear2DStamped acceleration(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000"));
+ acceleration.ax() = 1.5;
+ acceleration.ay() = -3.0;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ acceleration.data(),
+ acceleration.size(),
+ acceleration.localParameterization());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(acceleration.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(3.0, acceleration.ax(), 1.0e-5);
+ EXPECT_NEAR(-8.0, acceleration.ay(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp
new file mode 100644
index 000000000..a49709a1b
--- /dev/null
+++ b/fuse_variables/test/test_fixed_size_variable.cpp
@@ -0,0 +1,100 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+
+
+class TestVariable : public fuse_variables::FixedSizeVariable<2>
+{
+public:
+ SMART_PTR_DEFINITIONS(TestVariable);
+
+ TestVariable() :
+ uuid_(fuse_core::uuid::generate())
+ {}
+ virtual ~TestVariable() = default;
+
+ fuse_core::UUID uuid() const override { return uuid_; }
+ void print(std::ostream& stream = std::cout) const override {}
+ fuse_core::Variable::UniquePtr clone() const override { return TestVariable::make_unique(*this); }
+private:
+ fuse_core::UUID uuid_;
+};
+
+
+TEST(FixedSizeVariable, Size)
+{
+ // Verify the expected size is returned
+ TestVariable variable;
+ EXPECT_EQ(2, variable.size()); // base class interface
+ EXPECT_EQ(2ul, TestVariable::SIZE); // static member variable
+}
+
+TEST(FixedSizeVariable, Data)
+{
+ // Verify access to the data methods
+ TestVariable variable;
+ EXPECT_NO_THROW(variable.data()[0] = 1.0);
+ EXPECT_NO_THROW(variable.data()[1] = 2.0);
+ const TestVariable& const_variable = variable;
+ bool success = true;
+ EXPECT_NO_THROW(success = success && const_variable.data()[0] == 1.0);
+ EXPECT_NO_THROW(success = success && const_variable.data()[1] == 2.0);
+ EXPECT_TRUE(success);
+}
+
+TEST(FixedSizeVariable, Array)
+{
+ // Verify access to the array
+ TestVariable variable;
+ EXPECT_NO_THROW(variable.array()[0] = 1.0);
+ EXPECT_NO_THROW(variable.array().at(1) = 2.0);
+ EXPECT_NO_THROW(variable.array().front() = 3.0);
+ EXPECT_NO_THROW(variable.array().back() = 4.0);
+ const TestVariable& const_variable = variable;
+ bool success = true;
+ EXPECT_NO_THROW(success = success && const_variable.array()[0] == 3.0);
+ EXPECT_NO_THROW(success = success && const_variable.array().at(1) == 4.0);
+ EXPECT_NO_THROW(success = success && const_variable.array().front() == 3.0);
+ EXPECT_NO_THROW(success = success && const_variable.array().back() == 4.0);
+ EXPECT_TRUE(success);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp
new file mode 100644
index 000000000..b94091b97
--- /dev/null
+++ b/fuse_variables/test/test_orientation_2d_stamped.cpp
@@ -0,0 +1,130 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::Orientation2DStamped;
+
+
+TEST(Orientation2DStamped, Type)
+{
+ Orientation2DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::Orientation2DStamped", variable.type());
+}
+
+TEST(Orientation2DStamped, UUID)
+{
+ // Verify two velocities at the same timestamp produce the same UUID
+ {
+ Orientation2DStamped variable1(ros::Time(12345678, 910111213));
+ Orientation2DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+
+ Orientation2DStamped variable3(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ Orientation2DStamped variable4(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ EXPECT_EQ(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two velocities at different timestamps produce different UUIDs
+ {
+ Orientation2DStamped variable1(ros::Time(12345678, 910111213));
+ Orientation2DStamped variable2(ros::Time(12345678, 910111214));
+ Orientation2DStamped variable3(ros::Time(12345679, 910111213));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ EXPECT_NE(variable1.uuid(), variable3.uuid());
+ EXPECT_NE(variable2.uuid(), variable3.uuid());
+ }
+
+ // Verify two velocities with different hardware IDs produce different UUIDs
+ {
+ Orientation2DStamped variable1(ros::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2"));
+ Orientation2DStamped variable2(ros::Time(12345678, 910111213), fuse_core::uuid::generate("bb8"));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(3.0);
+ return true;
+ }
+};
+
+TEST(Orientation2DStamped, Optimization)
+{
+ // Create a Orientation2DStamped
+ Orientation2DStamped orientation(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000"));
+ orientation.theta() = 1.5;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ orientation.data(),
+ orientation.size(),
+ orientation.localParameterization());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(orientation.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(3.0, orientation.theta(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp
new file mode 100644
index 000000000..ec3cc92cd
--- /dev/null
+++ b/fuse_variables/test/test_position_2d_stamped.cpp
@@ -0,0 +1,133 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::Position2DStamped;
+
+
+TEST(Position2DStamped, Type)
+{
+ Position2DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::Position2DStamped", variable.type());
+}
+
+TEST(Position2DStamped, UUID)
+{
+ // Verify two velocities at the same timestamp produce the same UUID
+ {
+ Position2DStamped variable1(ros::Time(12345678, 910111213));
+ Position2DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+
+ Position2DStamped variable3(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ Position2DStamped variable4(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ EXPECT_EQ(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two velocities at different timestamps produce different UUIDs
+ {
+ Position2DStamped variable1(ros::Time(12345678, 910111213));
+ Position2DStamped variable2(ros::Time(12345678, 910111214));
+ Position2DStamped variable3(ros::Time(12345679, 910111213));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ EXPECT_NE(variable1.uuid(), variable3.uuid());
+ EXPECT_NE(variable2.uuid(), variable3.uuid());
+ }
+
+ // Verify two velocities with different hardware IDs produce different UUIDs
+ {
+ Position2DStamped variable1(ros::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2"));
+ Position2DStamped variable2(ros::Time(12345678, 910111213), fuse_core::uuid::generate("bb8"));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(3.0);
+ residual[1] = x[1] + T(8.0);
+ return true;
+ }
+};
+
+TEST(Position2DStamped, Optimization)
+{
+ // Create a Position2DStamped
+ Position2DStamped position(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000"));
+ position.x() = 1.5;
+ position.y() = -3.0;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ position.data(),
+ position.size(),
+ position.localParameterization());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(position.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(3.0, position.x(), 1.0e-5);
+ EXPECT_NEAR(-8.0, position.y(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp
new file mode 100644
index 000000000..2d5a978c8
--- /dev/null
+++ b/fuse_variables/test/test_position_3d_stamped.cpp
@@ -0,0 +1,153 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::Position3DStamped;
+
+
+TEST(Position3DStamped, Type)
+{
+ Position3DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::Position3DStamped", variable.type());
+}
+
+TEST(Position3DStamped, UUID)
+{
+ // Verify two positions at the same timestamp produce the same UUID
+ {
+ Position3DStamped variable1(ros::Time(12345678, 910111213));
+ Position3DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+ }
+
+ auto uuid_1 = fuse_core::uuid::generate("test_hardware1");
+ auto uuid_2 = fuse_core::uuid::generate("test_hardware2");
+
+ // Verify two positions at the same timestamp and same hardware ID produce the same UUID
+ {
+ Position3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
+ Position3DStamped variable2(ros::Time(12345678, 910111213), uuid_1);
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+ }
+
+ // Verify two positions with the same timestamp but different hardware IDs generate different UUIDs
+ {
+ Position3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
+ Position3DStamped variable2(ros::Time(12345678, 910111213), uuid_2);
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+
+ // Verify two positions with the same hardware ID and different timestamps produce different UUIDs
+ {
+ Position3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
+ Position3DStamped variable2(ros::Time(12345678, 910111214), uuid_1);
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+
+ Position3DStamped variable3(ros::Time(12345678, 910111213), uuid_1);
+ Position3DStamped variable4(ros::Time(12345679, 910111213), uuid_1);
+ EXPECT_NE(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two positions with different hardware IDs and different timestamps produce different UUIDs
+ {
+ Position3DStamped variable1(ros::Time(12345678, 910111213), uuid_1);
+ Position3DStamped variable2(ros::Time(12345678, 910111214), uuid_2);
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+
+ Position3DStamped variable3(ros::Time(12345678, 910111213), uuid_1);
+ Position3DStamped variable4(ros::Time(12345679, 910111213), uuid_2);
+ EXPECT_NE(variable3.uuid(), variable4.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(3.0);
+ residual[1] = x[1] + T(8.0);
+ residual[2] = x[2] - T(3.1);
+ return true;
+ }
+};
+
+TEST(Position3DStamped, Optimization)
+{
+ // Create a Position3DStamped
+ Position3DStamped position(ros::Time(12345678, 910111213));
+ position.x() = 1.5;
+ position.y() = -3.0;
+ position.z() = 0.8;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ position.data(),
+ position.size());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(position.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(3.0, position.x(), 1.0e-5);
+ EXPECT_NEAR(-8.0, position.y(), 1.0e-5);
+ EXPECT_NEAR(3.1, position.z(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp
new file mode 100644
index 000000000..c8f3b3b79
--- /dev/null
+++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp
@@ -0,0 +1,130 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::VelocityAngular2DStamped;
+
+
+TEST(VelocityAngular2DStamped, Type)
+{
+ VelocityAngular2DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::VelocityAngular2DStamped", variable.type());
+}
+
+TEST(VelocityAngular2DStamped, UUID)
+{
+ // Verify two velocities at the same timestamp produce the same UUID
+ {
+ VelocityAngular2DStamped variable1(ros::Time(12345678, 910111213));
+ VelocityAngular2DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+
+ VelocityAngular2DStamped variable3(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ VelocityAngular2DStamped variable4(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ EXPECT_EQ(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two velocities at different timestamps produce different UUIDs
+ {
+ VelocityAngular2DStamped variable1(ros::Time(12345678, 910111213));
+ VelocityAngular2DStamped variable2(ros::Time(12345678, 910111214));
+ VelocityAngular2DStamped variable3(ros::Time(12345679, 910111213));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ EXPECT_NE(variable1.uuid(), variable3.uuid());
+ EXPECT_NE(variable2.uuid(), variable3.uuid());
+ }
+
+ // Verify two velocities with different hardware IDs produce different UUIDs
+ {
+ VelocityAngular2DStamped variable1(ros::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2"));
+ VelocityAngular2DStamped variable2(ros::Time(12345678, 910111213), fuse_core::uuid::generate("bb8"));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(2.7);
+ return true;
+ }
+};
+
+TEST(VelocityAngular2DStamped, Optimization)
+{
+ // Create a VelocityAngular2DStamped
+ VelocityAngular2DStamped velocity(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000"));
+ velocity.vtheta() = 1.5;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ velocity.data(),
+ velocity.size(),
+ velocity.localParameterization());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(velocity.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(2.7, velocity.vtheta(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp
new file mode 100644
index 000000000..3b267c8e8
--- /dev/null
+++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp
@@ -0,0 +1,133 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Locus Robotics
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+using fuse_variables::VelocityLinear2DStamped;
+
+
+TEST(VelocityLinear2DStamped, Type)
+{
+ VelocityLinear2DStamped variable(ros::Time(12345678, 910111213));
+ EXPECT_EQ("fuse_variables::VelocityLinear2DStamped", variable.type());
+}
+
+TEST(VelocityLinear2DStamped, UUID)
+{
+ // Verify two velocities at the same timestamp produce the same UUID
+ {
+ VelocityLinear2DStamped variable1(ros::Time(12345678, 910111213));
+ VelocityLinear2DStamped variable2(ros::Time(12345678, 910111213));
+ EXPECT_EQ(variable1.uuid(), variable2.uuid());
+
+ VelocityLinear2DStamped variable3(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ VelocityLinear2DStamped variable4(ros::Time(12345678, 910111213), fuse_core::uuid::generate("c3po"));
+ EXPECT_EQ(variable3.uuid(), variable4.uuid());
+ }
+
+ // Verify two velocities at different timestamps produce different UUIDs
+ {
+ VelocityLinear2DStamped variable1(ros::Time(12345678, 910111213));
+ VelocityLinear2DStamped variable2(ros::Time(12345678, 910111214));
+ VelocityLinear2DStamped variable3(ros::Time(12345679, 910111213));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ EXPECT_NE(variable1.uuid(), variable3.uuid());
+ EXPECT_NE(variable2.uuid(), variable3.uuid());
+ }
+
+ // Verify two velocities with different hardware IDs produce different UUIDs
+ {
+ VelocityLinear2DStamped variable1(ros::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2"));
+ VelocityLinear2DStamped variable2(ros::Time(12345678, 910111213), fuse_core::uuid::generate("bb8"));
+ EXPECT_NE(variable1.uuid(), variable2.uuid());
+ }
+}
+
+struct CostFunctor
+{
+ CostFunctor() {}
+
+ template bool operator()(const T* const x, T* residual) const
+ {
+ residual[0] = x[0] - T(3.0);
+ residual[1] = x[1] + T(8.0);
+ return true;
+ }
+};
+
+TEST(VelocityLinear2DStamped, Optimization)
+{
+ // Create a VelocityLinear2DStamped
+ VelocityLinear2DStamped velocity(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000"));
+ velocity.vx() = 1.5;
+ velocity.vy() = -3.0;
+
+ // Create a simple a constraint
+ ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor());
+
+ // Build the problem.
+ ceres::Problem problem;
+ problem.AddParameterBlock(
+ velocity.data(),
+ velocity.size(),
+ velocity.localParameterization());
+ std::vector parameter_blocks;
+ parameter_blocks.push_back(velocity.data());
+ problem.AddResidualBlock(
+ cost_function,
+ nullptr,
+ parameter_blocks);
+
+ // Run the solver
+ ceres::Solver::Options options;
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, &problem, &summary);
+
+ // Check
+ EXPECT_NEAR(3.0, velocity.vx(), 1.0e-5);
+ EXPECT_NEAR(-8.0, velocity.vy(), 1.0e-5);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}