diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index aa6b02c54..b1e55c076 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -118,4 +118,28 @@ A constraint that represents a measurement on the difference between two 3D poses. + + + A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of + the 3D angular acceleration. + + + + + A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of + the 3D linear acceleration. + + + + + A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of + the 3D angular velocity. + + + + + A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of + the 3D linear velocity. + + diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 8bb4ac4c7..972819e5f 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -206,6 +206,14 @@ using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationAngular3DStampedConstraint = + AbsoluteConstraint; +using AbsoluteAccelerationLinear3DStampedConstraint = + AbsoluteConstraint; +using AbsoluteVelocityAngular3DStampedConstraint = + AbsoluteConstraint; +using AbsoluteVelocityLinear3DStampedConstraint = + AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation @@ -218,5 +226,9 @@ BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index cbb689b54..8e2c269db 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -185,6 +185,30 @@ inline std::string AbsoluteConstraint:: return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint"; } +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints_3d::AbsoluteAccelerationAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints_3d::AbsoluteAccelerationLinear3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints_3d::AbsoluteVelocityAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints_3d::AbsoluteVelocityLinear3DStampedConstraint"; +} + } // namespace fuse_constraints #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_IMPL_HPP_ diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index 9aa7593fc..bb32540bf 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -42,6 +42,10 @@ BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition2DStampedConstrai BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, @@ -64,3 +68,15 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, + fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, + fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, + fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, + fuse_core::Constraint);