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(); +}