diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h index 6f3f68f548d..634f5a43650 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_factories.h @@ -52,25 +52,29 @@ namespace tesseract_collision::tesseract_collision_bullet class BulletDiscreteBVHManagerFactory : public DiscreteContactManagerFactory { public: - DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& name, + const YAML::Node& config) const override final; }; class BulletDiscreteSimpleManagerFactory : public DiscreteContactManagerFactory { public: - DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& name, + const YAML::Node& config) const override final; }; class BulletCastBVHManagerFactory : public ContinuousContactManagerFactory { public: - ContinuousContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& name, + const YAML::Node& config) const override final; }; class BulletCastSimpleManagerFactory : public ContinuousContactManagerFactory { public: - ContinuousContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& name, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(BulletFactoriesAnchor) diff --git a/tesseract_collision/bullet/src/bullet_factories.cpp b/tesseract_collision/bullet/src/bullet_factories.cpp index 860e43e708e..565f66f549b 100644 --- a/tesseract_collision/bullet/src/bullet_factories.cpp +++ b/tesseract_collision/bullet/src/bullet_factories.cpp @@ -36,6 +36,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include + namespace tesseract_collision::tesseract_collision_bullet { TesseractCollisionConfigurationInfo getConfigInfo(const YAML::Node& config) @@ -59,26 +62,26 @@ TesseractCollisionConfigurationInfo getConfigInfo(const YAML::Node& config) return config_info; } -DiscreteContactManager::UPtr BulletDiscreteBVHManagerFactory::create(const std::string& name, - const YAML::Node& config) const +std::unique_ptr +BulletDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Node& config) const { return std::make_unique(name, getConfigInfo(config)); } -DiscreteContactManager::UPtr BulletDiscreteSimpleManagerFactory::create(const std::string& name, - const YAML::Node& config) const +std::unique_ptr BulletDiscreteSimpleManagerFactory::create(const std::string& name, + const YAML::Node& config) const { return std::make_unique(name, getConfigInfo(config)); } -ContinuousContactManager::UPtr BulletCastBVHManagerFactory::create(const std::string& name, - const YAML::Node& config) const +std::unique_ptr BulletCastBVHManagerFactory::create(const std::string& name, + const YAML::Node& config) const { return std::make_unique(name, getConfigInfo(config)); } -ContinuousContactManager::UPtr BulletCastSimpleManagerFactory::create(const std::string& name, - const YAML::Node& config) const +std::unique_ptr BulletCastSimpleManagerFactory::create(const std::string& name, + const YAML::Node& config) const { return std::make_unique(name, getConfigInfo(config)); } diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index 625b0a73d79..a2b67392272 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -221,7 +221,7 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: managed_shapes.resize(octree.getTreeDepth() + 1); switch (geom->getSubType()) { - case tesseract_geometry::Octree::SubType::BOX: + case tesseract_geometry::OctreeSubType::BOX: { for (auto it = octree.begin(static_cast(octree.getTreeDepth())), end = octree.end(); it != end; ++it) @@ -257,7 +257,7 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: return subshape; } - case tesseract_geometry::Octree::SubType::SPHERE_INSIDE: + case tesseract_geometry::OctreeSubType::SPHERE_INSIDE: { for (auto it = octree.begin(static_cast(octree.getTreeDepth())), end = octree.end(); it != end; ++it) @@ -292,7 +292,7 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: return subshape; } - case tesseract_geometry::Octree::SubType::SPHERE_OUTSIDE: + case tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE: { for (auto it = octree.begin(static_cast(octree.getTreeDepth())), end = octree.end(); it != end; ++it) diff --git a/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h b/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h index 0c9b559665e..82057e5acaf 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h +++ b/tesseract_collision/core/include/tesseract_collision/core/contact_managers_plugin_factory.h @@ -31,13 +31,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include #include -#include +#include // clang-format off #define TESSERACT_ADD_DISCRETE_MANAGER_PLUGIN(DERIVED_CLASS, ALIAS) \ @@ -51,6 +49,8 @@ namespace tesseract_collision { /** @brief Forward declare Plugin Factory */ class ContactManagersPluginFactory; +class DiscreteContactManager; +class ContinuousContactManager; /** @brief Define a discrete contact manager plugin which the factory can create an instance */ class DiscreteContactManagerFactory @@ -66,7 +66,7 @@ class DiscreteContactManagerFactory * @param name The name of the contact manager object * @return If failed to create, nullptr is returned. */ - virtual DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const = 0; + virtual std::unique_ptr create(const std::string& name, const YAML::Node& config) const = 0; protected: static const std::string SECTION_NAME; @@ -87,7 +87,8 @@ class ContinuousContactManagerFactory * @param name The name of the contact manager object * @return If failed to create, nullptr is returned. */ - virtual ContinuousContactManager::UPtr create(const std::string& solver_name, const YAML::Node& config) const = 0; + virtual std::unique_ptr create(const std::string& solver_name, + const YAML::Node& config) const = 0; protected: static const std::string SECTION_NAME; @@ -237,30 +238,30 @@ class ContactManagersPluginFactory * @details This looks for discrete contact manager plugin info. If not found nullptr is returned. * @param name The name */ - DiscreteContactManager::UPtr createDiscreteContactManager(const std::string& name) const; + std::unique_ptr createDiscreteContactManager(const std::string& name) const; /** * @brief Get discrete contact manager object given plugin info * @param name The name * @param plugin_info The plugin information to create kinematics object */ - DiscreteContactManager::UPtr createDiscreteContactManager(const std::string& name, - const tesseract_common::PluginInfo& plugin_info) const; + std::unique_ptr + createDiscreteContactManager(const std::string& name, const tesseract_common::PluginInfo& plugin_info) const; /** * @brief Get continuous contact manager object given name * @details This looks for continuous contact manager plugin info. If not found nullptr is returned. * @param name The name */ - ContinuousContactManager::UPtr createContinuousContactManager(const std::string& name) const; + std::unique_ptr createContinuousContactManager(const std::string& name) const; /** * @brief Get continuous contact manager object given plugin info * @param name The name * @param plugin_info The plugin information to create kinematics object */ - ContinuousContactManager::UPtr createContinuousContactManager(const std::string& name, - const tesseract_common::PluginInfo& plugin_info) const; + std::unique_ptr + createContinuousContactManager(const std::string& name, const tesseract_common::PluginInfo& plugin_info) const; /** * @brief Save the plugin information to a yaml config file diff --git a/tesseract_collision/core/include/tesseract_collision/core/convex_decomposition.h b/tesseract_collision/core/include/tesseract_collision/core/convex_decomposition.h index 38fe7734fa8..45ed51531ff 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/convex_decomposition.h +++ b/tesseract_collision/core/include/tesseract_collision/core/convex_decomposition.h @@ -26,10 +26,9 @@ #ifndef TESSERACT_COLLISION_CONVEX_DECOMPOSITION_H #define TESSERACT_COLLISION_CONVEX_DECOMPOSITION_H -#include #include -#include -#include +#include +#include namespace tesseract_collision { @@ -53,8 +52,8 @@ class ConvexDecomposition * index * @return */ - virtual std::vector compute(const tesseract_common::VectorVector3d& vertices, - const Eigen::VectorXi& faces) const = 0; + virtual std::vector> + compute(const tesseract_common::VectorVector3d& vertices, const Eigen::VectorXi& faces) const = 0; }; } // namespace tesseract_collision diff --git a/tesseract_collision/core/include/tesseract_collision/core/fwd.h b/tesseract_collision/core/include/tesseract_collision/core/fwd.h new file mode 100644 index 00000000000..b5cd0717850 --- /dev/null +++ b/tesseract_collision/core/include/tesseract_collision/core/fwd.h @@ -0,0 +1,62 @@ +/** + * @file types.h + * @brief Tesseracts Collision Forward Declarations + * + * @author Levi Armstrong + * @date February 17, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COLLISION_CORE_TESSERACT_COLLISION_FWD_H +#define TESSERACT_COLLISION_CORE_TESSERACT_COLLISION_FWD_H + +namespace tesseract_collision +{ +// types.h +enum class ContinuousCollisionType; +enum class ContactTestType; +struct ContactResult; +class ContactResultMap; +struct ContactRequest; +struct ContactTestData; +enum class CollisionEvaluatorType; +enum class CollisionCheckProgramType; +enum class ACMOverrideType; +struct ContactManagerConfig; +struct CollisionCheckConfig; +struct ContactTrajectorySubstepResults; +struct ContactTrajectoryStepResults; +struct ContactTrajectoryResults; + +// contact_managers_plugin_factory.h +class DiscreteContactManagerFactory; +class ContinuousContactManagerFactory; +class ContactManagersPluginFactory; + +// convex_decomposition.h +class ConvexDecomposition; + +// discrete_contact_manager.h +class DiscreteContactManager; + +// continuous_contact_manager.h +class ContinuousContactManager; +} // namespace tesseract_collision + +#endif // TESSERACT_COLLISION_CORE_TESSERACT_COLLISION_FWD_H diff --git a/tesseract_collision/core/include/tesseract_collision/core/serialization.h b/tesseract_collision/core/include/tesseract_collision/core/serialization.h index 6d05218034d..dafb7a678d6 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/serialization.h +++ b/tesseract_collision/core/include/tesseract_collision/core/serialization.h @@ -26,17 +26,7 @@ #ifndef TESSERACT_COLLISION_SERIALIZATION_H #define TESSERACT_COLLISION_SERIALIZATION_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include +#include namespace boost::serialization { diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 9a8c87eef82..67c663f5c86 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -36,20 +36,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include -#include -#include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include +#include + namespace tesseract_collision { -using CollisionShapesConst = std::vector; -using CollisionShapeConstPtr = tesseract_geometry::Geometry::ConstPtr; -using CollisionShapePtr = tesseract_geometry::Geometry::Ptr; +using CollisionShapeConstPtr = std::shared_ptr; +using CollisionShapePtr = std::shared_ptr; +using CollisionShapesConst = std::vector; using CollisionMarginData = tesseract_common::CollisionMarginData; using CollisionMarginOverrideType = tesseract_common::CollisionMarginOverrideType; -using PairsCollisionMarginData = tesseract_common::PairsCollisionMarginData; +using PairsCollisionMarginData = + std::unordered_map, double, tesseract_common::PairHash>; /** * @brief Should return true if contact allowed, otherwise false. diff --git a/tesseract_collision/core/include/tesseract_collision/core/utils.h b/tesseract_collision/core/include/tesseract_collision/core/utils.h index dfaa3dc1107..ae009305b65 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/utils.h +++ b/tesseract_collision/core/include/tesseract_collision/core/utils.h @@ -28,8 +28,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_collision diff --git a/tesseract_collision/core/src/contact_managers_plugin_factory.cpp b/tesseract_collision/core/src/contact_managers_plugin_factory.cpp index 2333880bc49..7b556f228be 100644 --- a/tesseract_collision/core/src/contact_managers_plugin_factory.cpp +++ b/tesseract_collision/core/src/contact_managers_plugin_factory.cpp @@ -29,6 +29,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include #include #include @@ -202,7 +204,8 @@ std::string ContactManagersPluginFactory::getDefaultContinuousContactManagerPlug return continuous_plugin_info_.default_plugin; } -DiscreteContactManager::UPtr ContactManagersPluginFactory::createDiscreteContactManager(const std::string& name) const +std::unique_ptr +ContactManagersPluginFactory::createDiscreteContactManager(const std::string& name) const { auto cm_it = discrete_plugin_info_.plugins.find(name); if (cm_it == discrete_plugin_info_.plugins.end()) @@ -216,7 +219,7 @@ DiscreteContactManager::UPtr ContactManagersPluginFactory::createDiscreteContact return createDiscreteContactManager(name, cm_it->second); } -DiscreteContactManager::UPtr +std::unique_ptr ContactManagersPluginFactory::createDiscreteContactManager(const std::string& name, const tesseract_common::PluginInfo& plugin_info) const { @@ -244,7 +247,7 @@ ContactManagersPluginFactory::createDiscreteContactManager(const std::string& na // LCOV_EXCL_STOP } -ContinuousContactManager::UPtr +std::unique_ptr ContactManagersPluginFactory::createContinuousContactManager(const std::string& name) const { auto cm_it = continuous_plugin_info_.plugins.find(name); @@ -259,7 +262,7 @@ ContactManagersPluginFactory::createContinuousContactManager(const std::string& return createContinuousContactManager(name, cm_it->second); } -ContinuousContactManager::UPtr +std::unique_ptr ContactManagersPluginFactory::createContinuousContactManager(const std::string& name, const tesseract_common::PluginInfo& plugin_info) const { diff --git a/tesseract_collision/core/src/continuous_contact_manager.cpp b/tesseract_collision/core/src/continuous_contact_manager.cpp index f7b9bc9f734..3a1c1eca79b 100644 --- a/tesseract_collision/core/src/continuous_contact_manager.cpp +++ b/tesseract_collision/core/src/continuous_contact_manager.cpp @@ -24,7 +24,7 @@ * limitations under the License. */ -#include +#include #include namespace tesseract_collision diff --git a/tesseract_collision/core/src/serialization.cpp b/tesseract_collision/core/src/serialization.cpp index 41f31fb8b04..53b465eba7c 100644 --- a/tesseract_collision/core/src/serialization.cpp +++ b/tesseract_collision/core/src/serialization.cpp @@ -30,6 +30,10 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include #include #include #include @@ -39,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace boost::serialization { diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_factories.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_factories.h index 679549e4ce1..37f7ed431da 100644 --- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_factories.h +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_factories.h @@ -33,7 +33,8 @@ namespace tesseract_collision::tesseract_collision_fcl class FCLDiscreteBVHManagerFactory : public DiscreteContactManagerFactory { public: - DiscreteContactManager::UPtr create(const std::string& name, const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& name, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(FCLFactoriesAnchor) diff --git a/tesseract_collision/fcl/src/fcl_factories.cpp b/tesseract_collision/fcl/src/fcl_factories.cpp index 9f2b41031cc..56d714363f7 100644 --- a/tesseract_collision/fcl/src/fcl_factories.cpp +++ b/tesseract_collision/fcl/src/fcl_factories.cpp @@ -26,11 +26,12 @@ #include #include +#include namespace tesseract_collision::tesseract_collision_fcl { -DiscreteContactManager::UPtr FCLDiscreteBVHManagerFactory::create(const std::string& name, - const YAML::Node& /*config*/) const +std::unique_ptr +FCLDiscreteBVHManagerFactory::create(const std::string& name, const YAML::Node& /*config*/) const { return std::make_unique(name); } @@ -41,4 +42,4 @@ TESSERACT_PLUGIN_ANCHOR_IMPL(FCLFactoriesAnchor) // LCOV_EXCL_LINE // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_DISCRETE_MANAGER_PLUGIN(tesseract_collision::tesseract_collision_fcl::FCLDiscreteBVHManagerFactory, - FCLDiscreteBVHManagerFactory); + FCLDiscreteBVHManagerFactory) diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index 9fc8d974700..1daac09ebf7 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -137,7 +137,7 @@ CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Octree::Cons { switch (geom->getSubType()) { - case tesseract_geometry::Octree::SubType::BOX: + case tesseract_geometry::OctreeSubType::BOX: { return std::make_shared(geom->getOctree()); } diff --git a/tesseract_collision/test/contact_managers_factory_static_unit.cpp b/tesseract_collision/test/contact_managers_factory_static_unit.cpp index 0f9bf697c08..e928bb54c33 100644 --- a/tesseract_collision/test/contact_managers_factory_static_unit.cpp +++ b/tesseract_collision/test/contact_managers_factory_static_unit.cpp @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include using namespace tesseract_collision; diff --git a/tesseract_collision/test/contact_managers_factory_unit.cpp b/tesseract_collision/test/contact_managers_factory_unit.cpp index 21ca5ac842e..a6a3f50de1e 100644 --- a/tesseract_collision/test/contact_managers_factory_unit.cpp +++ b/tesseract_collision/test/contact_managers_factory_unit.cpp @@ -30,6 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include using namespace tesseract_collision; diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp index 3d61a8e44fc..17aac5747ad 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp @@ -22,7 +22,8 @@ inline void addCollisionObjects(T& checker) ///////////////////////////////////////////////////////////////// std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt"; auto ot = std::make_shared(path); - CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::Octree::BOX); + CollisionShapePtr dense_octomap = + std::make_shared(ot, tesseract_geometry::OctreeSubType::BOX); Eigen::Isometry3d octomap_pose; octomap_pose.setIdentity(); octomap_pose.translation() = Eigen::Vector3d(1.1, 0, 0); @@ -39,7 +40,7 @@ inline void addCollisionObjects(T& checker) ///////////////////////////////////////////////////////////////// auto ot_b = std::make_shared(path); CollisionShapePtr dense_octomap_b = - std::make_shared(ot_b, tesseract_geometry::Octree::BOX); + std::make_shared(ot_b, tesseract_geometry::OctreeSubType::BOX); Eigen::Isometry3d octomap_pose_b; octomap_pose_b.setIdentity(); octomap_pose_b.translation() = Eigen::Vector3d(-1.1, 0, 0); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp index 49cd324fd62..7e4575066ec 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp @@ -23,7 +23,8 @@ inline void addCollisionObjects(DiscreteContactManager& checker) ///////////////////////////////////////////////////////////////// std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt"; auto ot = std::make_shared(path); - CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::Octree::BOX); + CollisionShapePtr dense_octomap = + std::make_shared(ot, tesseract_geometry::OctreeSubType::BOX); Eigen::Isometry3d octomap_pose; octomap_pose.setIdentity(); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp index 37feda3b200..4a1d2188a85 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp @@ -23,7 +23,7 @@ inline void addCollisionObjects(T& checker) std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt"; auto ot = std::make_shared(path); CollisionShapePtr dense_octomap = - std::make_shared(ot, tesseract_geometry::Octree::SPHERE_OUTSIDE); + std::make_shared(ot, tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE); Eigen::Isometry3d octomap_pose; octomap_pose.setIdentity(); octomap_pose.translation() = Eigen::Vector3d(1.1, 0, 0); @@ -40,7 +40,7 @@ inline void addCollisionObjects(T& checker) ///////////////////////////////////////////////////////////////// auto ot_b = std::make_shared(path); CollisionShapePtr dense_octomap_b = - std::make_shared(ot_b, tesseract_geometry::Octree::SPHERE_INSIDE); + std::make_shared(ot_b, tesseract_geometry::OctreeSubType::SPHERE_INSIDE); Eigen::Isometry3d octomap_pose_b; octomap_pose_b.setIdentity(); octomap_pose_b.translation() = Eigen::Vector3d(-1.1, 0, 0); diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp index 94a73834161..4f280c7e51c 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp @@ -24,7 +24,8 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex ///////////////////////////////////////////////////////////////// std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/blender_monkey.bt"; auto ot = std::make_shared(path); - CollisionShapePtr dense_octomap = std::make_shared(ot, tesseract_geometry::Octree::BOX); + CollisionShapePtr dense_octomap = + std::make_shared(ot, tesseract_geometry::OctreeSubType::BOX); Eigen::Isometry3d octomap_pose; octomap_pose.setIdentity(); diff --git a/tesseract_collision/vhacd/include/tesseract_collision/vhacd/convex_decomposition_vhacd.h b/tesseract_collision/vhacd/include/tesseract_collision/vhacd/convex_decomposition_vhacd.h index 34262831407..f8a61531b23 100644 --- a/tesseract_collision/vhacd/include/tesseract_collision/vhacd/convex_decomposition_vhacd.h +++ b/tesseract_collision/vhacd/include/tesseract_collision/vhacd/convex_decomposition_vhacd.h @@ -72,8 +72,8 @@ class ConvexDecompositionVHACD : public ConvexDecomposition ConvexDecompositionVHACD() = default; ConvexDecompositionVHACD(const VHACDParameters& params); - std::vector compute(const tesseract_common::VectorVector3d& vertices, - const Eigen::VectorXi& faces) const override; + std::vector> compute(const tesseract_common::VectorVector3d& vertices, + const Eigen::VectorXi& faces) const override; private: VHACDParameters params_; diff --git a/tesseract_collision/vhacd/src/convex_decomposition_vhacd.cpp b/tesseract_collision/vhacd/src/convex_decomposition_vhacd.cpp index c27053f5277..5f88ee91fb3 100644 --- a/tesseract_collision/vhacd/src/convex_decomposition_vhacd.cpp +++ b/tesseract_collision/vhacd/src/convex_decomposition_vhacd.cpp @@ -1,10 +1,12 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_collision { @@ -31,7 +33,7 @@ class ProgressCallback : public VHACD::IVHACD::IUserCallback ConvexDecompositionVHACD::ConvexDecompositionVHACD(const VHACDParameters& params) : params_(params) {} -std::vector +std::vector > ConvexDecompositionVHACD::compute(const tesseract_common::VectorVector3d& vertices, const Eigen::VectorXi& faces) const { params_.print(); diff --git a/tesseract_collision/vhacd/src/create_convex_decomposition.cpp b/tesseract_collision/vhacd/src/create_convex_decomposition.cpp index 9862e65a92a..823759fe32b 100644 --- a/tesseract_collision/vhacd/src/create_convex_decomposition.cpp +++ b/tesseract_collision/vhacd/src/create_convex_decomposition.cpp @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace { @@ -159,7 +160,8 @@ int main(int argc, char** argv) } tesseract_collision::ConvexDecompositionVHACD convex_decomp(params); - std::vector convex_hulls = convex_decomp.compute(mesh_vertices, mesh_faces); + std::vector> convex_hulls = + convex_decomp.compute(mesh_vertices, mesh_faces); if (convex_hulls.empty()) { diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt index 07764854327..5dc4eb1dca9 100644 --- a/tesseract_common/CMakeLists.txt +++ b/tesseract_common/CMakeLists.txt @@ -46,14 +46,17 @@ add_library( ${PROJECT_NAME} src/allowed_collision_matrix.cpp src/any_poly.cpp + src/calibration_info.cpp src/collision_margin_data.cpp src/joint_state.cpp src/manipulator_info.cpp src/kinematic_limits.cpp src/eigen_serialization.cpp src/utils.cpp + src/plugin_info.cpp src/resource_locator.cpp - src/types.cpp) + src/types.cpp + src/timer.cpp) target_link_libraries( ${PROJECT_NAME} PUBLIC Eigen3::Eigen diff --git a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h index 50f307022fa..86d5ebd3e8b 100644 --- a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h +++ b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h @@ -43,11 +43,7 @@ class AllowedCollisionMatrix */ virtual void addAllowedCollision(const std::string& link_name1, const std::string& link_name2, - const std::string& reason) - { - auto link_pair = tesseract_common::makeOrderedLinkPair(link_name1, link_name2); - lookup_table_[link_pair] = reason; - } + const std::string& reason); /** * @brief Get all of the entries in the allowed collision matrix @@ -55,37 +51,20 @@ class AllowedCollisionMatrix * collision entries. The keys of the unordered map are a std::pair * of the link names in the allowed collision pair. */ - const AllowedCollisionEntries& getAllAllowedCollisions() const { return lookup_table_; } + const AllowedCollisionEntries& getAllAllowedCollisions() const; /** * @brief Remove disabled collision pair from allowed collision matrix * @param obj1 Collision object name * @param obj2 Collision object name */ - virtual void removeAllowedCollision(const std::string& link_name1, const std::string& link_name2) - { - auto link_pair = tesseract_common::makeOrderedLinkPair(link_name1, link_name2); - lookup_table_.erase(link_pair); - } + virtual void removeAllowedCollision(const std::string& link_name1, const std::string& link_name2); /** * @brief Remove disabled collision for any pair with link_name from allowed collision matrix * @param link_name Collision object name */ - virtual void removeAllowedCollision(const std::string& link_name) - { - for (auto it = lookup_table_.begin(); it != lookup_table_.end() /* not hoisted */; /* no increment */) - { - if (it->first.first == link_name || it->first.second == link_name) - { - it = lookup_table_.erase(it); - } - else - { - ++it; - } - } - } + virtual void removeAllowedCollision(const std::string& link_name); /** * @brief This checks if two links are allowed to be in collision @@ -93,40 +72,26 @@ class AllowedCollisionMatrix * @param link_name2 Second link anme * @return True if allowed to be in collision, otherwise false */ - virtual bool isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const - { - thread_local LinkNamesPair link_pair; - tesseract_common::makeOrderedLinkPair(link_pair, link_name1, link_name2); - return (lookup_table_.find(link_pair) != lookup_table_.end()); - } + virtual bool isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const; /** * @brief Clears the list of allowed collisions, so that no collision will be * allowed. */ - void clearAllowedCollisions() { lookup_table_.clear(); } + void clearAllowedCollisions(); /** * @brief Inserts an allowable collision matrix ignoring duplicate pairs * @param acm ACM to be inserted */ - void insertAllowedCollisionMatrix(const AllowedCollisionMatrix& acm) - { - lookup_table_.insert(acm.getAllAllowedCollisions().begin(), acm.getAllAllowedCollisions().end()); - } + void insertAllowedCollisionMatrix(const AllowedCollisionMatrix& acm); /** * @brief Reserve space for the internal data storage * @param size The size to reserve */ - void reserveAllowedCollisionMatrix(std::size_t size) { lookup_table_.reserve(size); } - - friend std::ostream& operator<<(std::ostream& os, const AllowedCollisionMatrix& acm) - { - for (const auto& pair : acm.getAllAllowedCollisions()) - os << "link=" << pair.first.first << " link=" << pair.first.second << " reason=" << pair.second << std::endl; - return os; - } + void reserveAllowedCollisionMatrix(std::size_t size); + bool operator==(const AllowedCollisionMatrix& rhs) const; bool operator!=(const AllowedCollisionMatrix& rhs) const; @@ -138,10 +103,10 @@ class AllowedCollisionMatrix void serialize(Archive& ar, const unsigned int version); // NOLINT }; +std::ostream& operator<<(std::ostream& os, const AllowedCollisionMatrix& acm); } // namespace tesseract_common #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_common::AllowedCollisionMatrix, "AllowedCollisionMatrix") #endif // TESSERACT_SCENE_GRAPH_ALLOWED_COLLISION_MATRIX_H diff --git a/tesseract_common/include/tesseract_common/calibration_info.h b/tesseract_common/include/tesseract_common/calibration_info.h new file mode 100644 index 00000000000..1eee43d930c --- /dev/null +++ b/tesseract_common/include/tesseract_common/calibration_info.h @@ -0,0 +1,79 @@ +/** + * @file calibration_info.h + * @brief Calibration Information + * + * @author Levi Armstrong + * @date January 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COMMON_CALIBRATION_INFO_H +#define TESSERACT_COMMON_CALIBRATION_INFO_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_common +{ +/** @brief The CalibrationInfo struct */ +struct CalibrationInfo +{ + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + + CalibrationInfo() = default; + + /** + * @brief The joint origin calibration information + * @details For each entry in \p joints the environment will apply a ChangeJointOriginCommand replacing the current + * joint origin with what is stored in the TransformMap + */ + tesseract_common::TransformMap joints; + + /** @brief Insert the content of an other CalibrationInfo */ + void insert(const CalibrationInfo& other); + + /** @brief Clear the contents */ + void clear(); + + /** @brief Check if structure is empty */ + bool empty() const; + + // Yaml Config key + static inline const std::string CONFIG_KEY{ "calibration" }; + + bool operator==(const CalibrationInfo& rhs) const; + bool operator!=(const CalibrationInfo& rhs) const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; +} // namespace tesseract_common + +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_common::CalibrationInfo, "CalibrationInfo") + +#endif // TESSERACT_COMMON_CALIBRATION_INFO_H diff --git a/tesseract_common/include/tesseract_common/collision_margin_data.h b/tesseract_common/include/tesseract_common/collision_margin_data.h index 6919ce5b083..33795d8efad 100644 --- a/tesseract_common/include/tesseract_common/collision_margin_data.h +++ b/tesseract_common/include/tesseract_common/collision_margin_data.h @@ -78,38 +78,21 @@ class CollisionMarginData using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - CollisionMarginData(double default_collision_margin = 0) - : default_collision_margin_(default_collision_margin), max_collision_margin_(default_collision_margin) - { - } - - CollisionMarginData(double default_collision_margin, PairsCollisionMarginData pair_collision_margins) - : default_collision_margin_(default_collision_margin), lookup_table_(std::move(pair_collision_margins)) - { - updateMaxCollisionMargin(); - } - - CollisionMarginData(PairsCollisionMarginData pair_collision_margins) - : lookup_table_(std::move(pair_collision_margins)) - { - updateMaxCollisionMargin(); - } + CollisionMarginData(double default_collision_margin = 0); + CollisionMarginData(double default_collision_margin, PairsCollisionMarginData pair_collision_margins); + CollisionMarginData(PairsCollisionMarginData pair_collision_margins); /** * @brief Set the default collision margin * @param default_collision_margin New default collision margin */ - void setDefaultCollisionMargin(double default_collision_margin) - { - default_collision_margin_ = default_collision_margin; - updateMaxCollisionMargin(); - } + void setDefaultCollisionMargin(double default_collision_margin); /** * @brief Get the default collision margin * @return default collision margin */ - double getDefaultCollisionMargin() const { return default_collision_margin_; } + double getDefaultCollisionMargin() const; /** * @brief Set the margin for a given contact pair @@ -121,12 +104,7 @@ class CollisionMarginData * @param obj2 The Second object name. Order doesn't matter * @param collision_margin contacts with distance < collision_margin are considered in collision */ - void setPairCollisionMargin(const std::string& obj1, const std::string& obj2, double collision_margin) - { - auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2); - lookup_table_[key] = collision_margin; - updateMaxCollisionMargin(); - } + void setPairCollisionMargin(const std::string& obj1, const std::string& obj2, double collision_margin); /** * @brief Get the pairs collision margin data @@ -137,23 +115,13 @@ class CollisionMarginData * @param obj2 The second object name * @return A Vector2d[Contact Distance Threshold, Coefficient] */ - double getPairCollisionMargin(const std::string& obj1, const std::string& obj2) const - { - thread_local LinkNamesPair key; - tesseract_common::makeOrderedLinkPair(key, obj1, obj2); - const auto it = lookup_table_.find(key); - - if (it != lookup_table_.end()) - return it->second; - - return default_collision_margin_; - } + double getPairCollisionMargin(const std::string& obj1, const std::string& obj2) const; /** * @brief Get Collision Margin Data for stored pairs * @return A map of link pairs collision margin data */ - const PairsCollisionMarginData& getPairCollisionMargins() const { return lookup_table_; } + const PairsCollisionMarginData& getPairCollisionMargins() const; /** * @brief Get the largest collision margin @@ -162,82 +130,26 @@ class CollisionMarginData * * @return Max contact distance threshold */ - double getMaxCollisionMargin() const { return max_collision_margin_; } + double getMaxCollisionMargin() const; /** * @brief Increment all margins by input amount. Useful for inflating or reducing margins * @param increment Amount to increment margins */ - void incrementMargins(const double& increment) - { - default_collision_margin_ += increment; - max_collision_margin_ += increment; - for (auto& pair : lookup_table_) - pair.second += increment; - } + void incrementMargins(const double& increment); /** * @brief Scale all margins by input value * @param scale Value by which all margins are multiplied */ - void scaleMargins(const double& scale) - { - default_collision_margin_ *= scale; - max_collision_margin_ *= scale; - for (auto& pair : lookup_table_) - pair.second *= scale; - } + void scaleMargins(const double& scale); /** * @brief Apply the contents of the provide CollisionMarginData based on the override type * @param collision_margin_data The collision margin data to apply * @param override_type The type indicating how the provided data should be applied. */ - void apply(const CollisionMarginData& collision_margin_data, CollisionMarginOverrideType override_type) - { - switch (override_type) - { - case CollisionMarginOverrideType::REPLACE: - { - *this = collision_margin_data; - break; - } - case CollisionMarginOverrideType::MODIFY: - { - default_collision_margin_ = collision_margin_data.default_collision_margin_; - - for (const auto& p : collision_margin_data.lookup_table_) - lookup_table_[p.first] = p.second; - - updateMaxCollisionMargin(); - break; - } - case CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN: - { - default_collision_margin_ = collision_margin_data.default_collision_margin_; - updateMaxCollisionMargin(); - break; - } - case CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN: - { - lookup_table_ = collision_margin_data.lookup_table_; - updateMaxCollisionMargin(); - break; - } - case CollisionMarginOverrideType::MODIFY_PAIR_MARGIN: - { - for (const auto& p : collision_margin_data.lookup_table_) - lookup_table_[p.first] = p.second; - - updateMaxCollisionMargin(); - break; - } - case CollisionMarginOverrideType::NONE: - { - break; - } - } - } + void apply(const CollisionMarginData& collision_margin_data, CollisionMarginOverrideType override_type); bool operator==(const CollisionMarginData& rhs) const; bool operator!=(const CollisionMarginData& rhs) const; @@ -253,15 +165,8 @@ class CollisionMarginData PairsCollisionMarginData lookup_table_; /** @brief Update the max collision margin */ - void updateMaxCollisionMargin() - { - max_collision_margin_ = default_collision_margin_; - for (const auto& p : lookup_table_) - { - if (p.second > max_collision_margin_) - max_collision_margin_ = p.second; - } - } + void updateMaxCollisionMargin(); + friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT diff --git a/tesseract_common/include/tesseract_common/eigen_types.h b/tesseract_common/include/tesseract_common/eigen_types.h new file mode 100644 index 00000000000..d2be253d94c --- /dev/null +++ b/tesseract_common/include/tesseract_common/eigen_types.h @@ -0,0 +1,37 @@ +#ifndef TESSERACT_COMMON_EIGEN_UTILS_H +#define TESSERACT_COMMON_EIGEN_UTILS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +namespace tesseract_common +{ +template +using AlignedVector = std::vector>; + +template +using AlignedMap = std::map, Eigen::aligned_allocator>>; + +template +using AlignedUnorderedMap = std::unordered_map, + std::equal_to, + Eigen::aligned_allocator>>; + +using VectorIsometry3d = AlignedVector; +using VectorVector4d = AlignedVector; +using VectorVector3d = std::vector; +using VectorVector2d = AlignedVector; +using TransformMap = AlignedMap; +using Toolpath = AlignedVector; +using TrajArray = Eigen::Matrix; +} // namespace tesseract_common + +#endif // TESSERACT_COLLISION_EIGEN_UTILS_H diff --git a/tesseract_common/include/tesseract_common/filesystem.h b/tesseract_common/include/tesseract_common/filesystem.h new file mode 100644 index 00000000000..e823134d00c --- /dev/null +++ b/tesseract_common/include/tesseract_common/filesystem.h @@ -0,0 +1,40 @@ +/** + * @file filesystem.h + * @brief Common Tesseract Filesystem + * + * @author Levi Armstrong + * @date January 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COMMON_FILESYSTEM_H +#define TESSERACT_COMMON_FILESYSTEM_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +namespace tesseract_common +{ +/** @brief Enable easy switching to std::filesystem when available */ +namespace fs = boost::filesystem; +} // namespace tesseract_common + +#endif // TESSERACT_COMMON_FILESYSTEM_H diff --git a/tesseract_common/include/tesseract_common/fwd.h b/tesseract_common/include/tesseract_common/fwd.h new file mode 100644 index 00000000000..9e5f7258eae --- /dev/null +++ b/tesseract_common/include/tesseract_common/fwd.h @@ -0,0 +1,75 @@ +/** + * @file tesseract_common_fwd.h + * @brief Tesseract common package forward declarations + * + * @author Levi Armstrong + * @author Matthew Powelson + * @date November 15, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COMMON_TESSERACT_COMMON_FWD_H +#define TESSERACT_COMMON_TESSERACT_COMMON_FWD_H + +namespace tesseract_common +{ +// types.h +struct PluginInfo; +struct PluginInfoContainer; +struct KinematicsPluginInfo; +struct ContactManagersPluginInfo; +struct TaskComposerPluginInfo; +struct CalibrationInfo; +struct PairHash; + +// any_poly.h +struct AnyPoly; + +// class_loader.h +struct ClassLoader; + +// allowed_collision_matrix +class AllowedCollisionMatrix; + +// collision_margin_data.h +enum class CollisionMarginOverrideType; +class CollisionMarginData; + +// joint_state.h +class JointState; +class JointTrajectory; + +// kinematic_limits.h +struct KinematicLimits; + +// manipulator_info.h +struct ManipulatorInfo; + +// plugin_loader.h +class PluginLoader; + +// resource_locator.h +class Resource; +class ResourceLocator; + +// timer.h +class Timer; +} // namespace tesseract_common + +#endif // TESSERACT_COMMON_TESSERACT_COMMON_FWD_H diff --git a/tesseract_common/include/tesseract_common/joint_state.h b/tesseract_common/include/tesseract_common/joint_state.h index 41d7465613c..eb6a0a25528 100644 --- a/tesseract_common/include/tesseract_common/joint_state.h +++ b/tesseract_common/include/tesseract_common/joint_state.h @@ -69,10 +69,6 @@ class JointState void serialize(Archive& ar, const unsigned int version); // NOLINT }; -} // namespace tesseract_common - -namespace tesseract_common -{ /** @brief Represents a joint trajectory */ class JointTrajectory { diff --git a/tesseract_common/include/tesseract_common/plugin_info.h b/tesseract_common/include/tesseract_common/plugin_info.h new file mode 100644 index 00000000000..ea4c86f9a2c --- /dev/null +++ b/tesseract_common/include/tesseract_common/plugin_info.h @@ -0,0 +1,205 @@ +/** + * @file plugin_info.h + * @brief Common Tesseract Plugin Infos + * + * @author Levi Armstrong + * @date January 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COMMON_PLUGIN_INFO_H +#define TESSERACT_COMMON_PLUGIN_INFO_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_common +{ +/** @brief The Plugin Information struct */ +// NOLINTNEXTLINE +struct PluginInfo +{ + /** @brief The plugin class name */ + std::string class_name; + + /** @brief The plugin config data */ + YAML::Node config; + + /** @brief Get the yaml config as a string */ + std::string getConfigString() const; + + bool operator==(const PluginInfo& rhs) const; + bool operator!=(const PluginInfo& rhs) const; + +private: + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int version) const; // NOLINT + + template + void load(Archive& ar, const unsigned int version); // NOLINT + + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +/** @brief A map of PluginInfo to user defined name */ +using PluginInfoMap = std::map; + +struct PluginInfoContainer +{ + std::string default_plugin; + PluginInfoMap plugins; + void clear(); + + bool operator==(const PluginInfoContainer& rhs) const; + bool operator!=(const PluginInfoContainer& rhs) const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +/** @brief The kinematics plugin information structure */ +struct KinematicsPluginInfo +{ + /** @brief A list of paths to search for plugins */ + std::set search_paths; + + /** @brief A list of library names without the prefix or suffix that contain plugins*/ + std::set search_libraries; + + /** @brief A map of group name to forward kinematics plugin information */ + std::map fwd_plugin_infos; + + /** @brief A map of group name to inverse kinematics plugin information */ + std::map inv_plugin_infos; + + /** @brief Insert the content of an other KinematicsPluginInfo */ + void insert(const KinematicsPluginInfo& other); + + /** @brief Clear the contents */ + void clear(); + + /** @brief Check if structure is empty */ + bool empty() const; + + // Yaml Config key + static inline const std::string CONFIG_KEY{ "kinematic_plugins" }; + + bool operator==(const KinematicsPluginInfo& rhs) const; + bool operator!=(const KinematicsPluginInfo& rhs) const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +/** @brief The contact managers plugin information structure */ +struct ContactManagersPluginInfo +{ + /** @brief A list of paths to search for plugins */ + std::set search_paths; + + /** @brief A list of library names without the prefix or suffix that contain plugins*/ + std::set search_libraries; + + /** @brief A map of name to discrete contact manager plugin information */ + tesseract_common::PluginInfoContainer discrete_plugin_infos; + + /** @brief A map of name to continuous contact manager plugin information */ + tesseract_common::PluginInfoContainer continuous_plugin_infos; + + /** @brief Insert the content of an other ContactManagersPluginInfo */ + void insert(const ContactManagersPluginInfo& other); + + /** @brief Clear the contents */ + void clear(); + + /** @brief Check if structure is empty */ + bool empty() const; + + // Yaml Config key + static inline const std::string CONFIG_KEY{ "contact_manager_plugins" }; + + bool operator==(const ContactManagersPluginInfo& rhs) const; + bool operator!=(const ContactManagersPluginInfo& rhs) const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +/** @brief The task composer plugin information structure */ +struct TaskComposerPluginInfo +{ + /** @brief A list of paths to search for plugins */ + std::set search_paths; + + /** @brief A list of library names without the prefix or suffix that contain plugins*/ + std::set search_libraries; + + /** @brief A map of name to task composer executor plugin information */ + tesseract_common::PluginInfoContainer executor_plugin_infos; + + /** @brief A map of name to task composer task plugin information */ + tesseract_common::PluginInfoContainer task_plugin_infos; + + /** @brief Insert the content of an other TaskComposerPluginInfo */ + void insert(const TaskComposerPluginInfo& other); + + /** @brief Clear the contents */ + void clear(); + + /** @brief Check if structure is empty */ + bool empty() const; + + // Yaml Config key + static inline const std::string CONFIG_KEY{ "task_composer_plugins" }; + + bool operator==(const TaskComposerPluginInfo& rhs) const; + bool operator!=(const TaskComposerPluginInfo& rhs) const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +} // namespace tesseract_common + +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_common::PluginInfo, "PluginInfo") +BOOST_CLASS_EXPORT_KEY2(tesseract_common::PluginInfoContainer, "PluginInfoContainer") +BOOST_CLASS_EXPORT_KEY2(tesseract_common::KinematicsPluginInfo, "KinematicsPluginInfo") +BOOST_CLASS_EXPORT_KEY2(tesseract_common::ContactManagersPluginInfo, "ContactManagersPluginInfo") +BOOST_CLASS_EXPORT_KEY2(tesseract_common::TaskComposerPluginInfo, "TaskComposerPluginInfo") + +#endif // TESSERACT_COMMON_PLUGIN_INFO_H diff --git a/tesseract_common/include/tesseract_common/serialization.h b/tesseract_common/include/tesseract_common/serialization.h index 4923e1ee074..bce0ab5c27f 100644 --- a/tesseract_common/include/tesseract_common/serialization.h +++ b/tesseract_common/include/tesseract_common/serialization.h @@ -38,7 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include // Used to replace commas in these macros to avoid them being interpreted as multiple arguments diff --git a/tesseract_common/include/tesseract_common/timer.h b/tesseract_common/include/tesseract_common/timer.h index 56ca53a44eb..a4f3e952963 100644 --- a/tesseract_common/include/tesseract_common/timer.h +++ b/tesseract_common/include/tesseract_common/timer.h @@ -40,38 +40,24 @@ class Timer public: /** @brief Start the timer */ - void start() - { - start_time_ = Clock::now(); - running_ = true; - } + void start(); /** @brief Stop the timer */ - void stop() - { - end_time_ = Clock::now(); - running_ = false; - } + void stop(); /** * @brief Get the elapsed time in milliseconds * @details If timer is actively running it will use Clock::now() as the end time * @return The elapsed time in milliseconds */ - double elapsedMilliseconds() const - { - if (running_) - return std::chrono::duration(Clock::now() - start_time_).count(); - - return std::chrono::duration(end_time_ - start_time_).count(); - } + double elapsedMilliseconds() const; /** * @brief Get the elapsed time in seconds * @details If timer is actively running it will use Clock::now() as the end time * @return The elapsed time in seconds */ - double elapsedSeconds() const { return (elapsedMilliseconds() / 1000.0); } + double elapsedSeconds() const; private: std::chrono::time_point start_time_; diff --git a/tesseract_common/include/tesseract_common/types.h b/tesseract_common/include/tesseract_common/types.h index 68958df59e7..e729b5877f2 100644 --- a/tesseract_common/include/tesseract_common/types.h +++ b/tesseract_common/include/tesseract_common/types.h @@ -28,45 +28,12 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - namespace tesseract_common { -/** @brief Enable easy switching to std::filesystem when available */ -namespace fs = boost::filesystem; - -template -using AlignedVector = std::vector>; - -template -using AlignedMap = std::map, Eigen::aligned_allocator>>; - -template -using AlignedUnorderedMap = std::unordered_map, - std::equal_to, - Eigen::aligned_allocator>>; - -using VectorIsometry3d = AlignedVector; -using VectorVector4d = AlignedVector; -using VectorVector3d = std::vector; -using VectorVector2d = AlignedVector; -using TransformMap = AlignedMap; -using Toolpath = AlignedVector; - -using TrajArray = Eigen::Matrix; - using LinkNamesPair = std::pair; struct PairHash @@ -97,205 +64,6 @@ LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, const std::stri */ void makeOrderedLinkPair(LinkNamesPair& pair, const std::string& link_name1, const std::string& link_name2); -/** @brief The Plugin Information struct */ -// NOLINTNEXTLINE -struct PluginInfo -{ - /** @brief The plugin class name */ - std::string class_name; - - /** @brief The plugin config data */ - YAML::Node config; - - /** @brief Get the yaml config as a string */ - std::string getConfigString() const; - - bool operator==(const PluginInfo& rhs) const; - bool operator!=(const PluginInfo& rhs) const; - -private: - friend class boost::serialization::access; - template - void save(Archive& ar, const unsigned int version) const; // NOLINT - - template - void load(Archive& ar, const unsigned int version); // NOLINT - - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; - -/** @brief A map of PluginInfo to user defined name */ -using PluginInfoMap = std::map; - -struct PluginInfoContainer -{ - std::string default_plugin; - PluginInfoMap plugins; - void clear(); - - bool operator==(const PluginInfoContainer& rhs) const; - bool operator!=(const PluginInfoContainer& rhs) const; - -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; - -/** @brief The kinematics plugin information structure */ -struct KinematicsPluginInfo -{ - /** @brief A list of paths to search for plugins */ - std::set search_paths; - - /** @brief A list of library names without the prefix or suffix that contain plugins*/ - std::set search_libraries; - - /** @brief A map of group name to forward kinematics plugin information */ - std::map fwd_plugin_infos; - - /** @brief A map of group name to inverse kinematics plugin information */ - std::map inv_plugin_infos; - - /** @brief Insert the content of an other KinematicsPluginInfo */ - void insert(const KinematicsPluginInfo& other); - - /** @brief Clear the contents */ - void clear(); - - /** @brief Check if structure is empty */ - bool empty() const; - - // Yaml Config key - static inline const std::string CONFIG_KEY{ "kinematic_plugins" }; - - bool operator==(const KinematicsPluginInfo& rhs) const; - bool operator!=(const KinematicsPluginInfo& rhs) const; - -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; - -/** @brief The contact managers plugin information structure */ -struct ContactManagersPluginInfo -{ - /** @brief A list of paths to search for plugins */ - std::set search_paths; - - /** @brief A list of library names without the prefix or suffix that contain plugins*/ - std::set search_libraries; - - /** @brief A map of name to discrete contact manager plugin information */ - tesseract_common::PluginInfoContainer discrete_plugin_infos; - - /** @brief A map of name to continuous contact manager plugin information */ - tesseract_common::PluginInfoContainer continuous_plugin_infos; - - /** @brief Insert the content of an other ContactManagersPluginInfo */ - void insert(const ContactManagersPluginInfo& other); - - /** @brief Clear the contents */ - void clear(); - - /** @brief Check if structure is empty */ - bool empty() const; - - // Yaml Config key - static inline const std::string CONFIG_KEY{ "contact_manager_plugins" }; - - bool operator==(const ContactManagersPluginInfo& rhs) const; - bool operator!=(const ContactManagersPluginInfo& rhs) const; - -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; - -/** @brief The task composer plugin information structure */ -struct TaskComposerPluginInfo -{ - /** @brief A list of paths to search for plugins */ - std::set search_paths; - - /** @brief A list of library names without the prefix or suffix that contain plugins*/ - std::set search_libraries; - - /** @brief A map of name to task composer executor plugin information */ - tesseract_common::PluginInfoContainer executor_plugin_infos; - - /** @brief A map of name to task composer task plugin information */ - tesseract_common::PluginInfoContainer task_plugin_infos; - - /** @brief Insert the content of an other TaskComposerPluginInfo */ - void insert(const TaskComposerPluginInfo& other); - - /** @brief Clear the contents */ - void clear(); - - /** @brief Check if structure is empty */ - bool empty() const; - - // Yaml Config key - static inline const std::string CONFIG_KEY{ "task_composer_plugins" }; - - bool operator==(const TaskComposerPluginInfo& rhs) const; - bool operator!=(const TaskComposerPluginInfo& rhs) const; - -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; - -/** @brief The CalibrationInfo struct */ -struct CalibrationInfo -{ - // LCOV_EXCL_START - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - // LCOV_EXCL_STOP - - CalibrationInfo() = default; - - /** - * @brief The joint origin calibration information - * @details For each entry in \p joints the environment will apply a ChangeJointOriginCommand replacing the current - * joint origin with what is stored in the TransformMap - */ - tesseract_common::TransformMap joints; - - /** @brief Insert the content of an other CalibrationInfo */ - void insert(const CalibrationInfo& other); - - /** @brief Clear the contents */ - void clear(); - - /** @brief Check if structure is empty */ - bool empty() const; - - // Yaml Config key - static inline const std::string CONFIG_KEY{ "calibration" }; - - bool operator==(const CalibrationInfo& rhs) const; - bool operator!=(const CalibrationInfo& rhs) const; - -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version); // NOLINT -}; } // namespace tesseract_common -#include -#include -BOOST_CLASS_EXPORT_KEY2(tesseract_common::PluginInfo, "PluginInfo") -BOOST_CLASS_EXPORT_KEY2(tesseract_common::PluginInfoContainer, "PluginInfoContainer") -BOOST_CLASS_EXPORT_KEY2(tesseract_common::KinematicsPluginInfo, "KinematicsPluginInfo") -BOOST_CLASS_EXPORT_KEY2(tesseract_common::ContactManagersPluginInfo, "ContactManagersPluginInfo") -BOOST_CLASS_EXPORT_KEY2(tesseract_common::TaskComposerPluginInfo, "TaskComposerPluginInfo") -BOOST_CLASS_EXPORT_KEY2(tesseract_common::CalibrationInfo, "CalibrationInfo") - #endif // TESSERACT_COMMON_TYPES_H diff --git a/tesseract_common/include/tesseract_common/utils.h b/tesseract_common/include/tesseract_common/utils.h index d118fe2a375..679ffb3337c 100644 --- a/tesseract_common/include/tesseract_common/utils.h +++ b/tesseract_common/include/tesseract_common/utils.h @@ -30,16 +30,23 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include #include #include #include #include -#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include + +namespace tinyxml2 +{ +class XMLElement; +class XMLAttribute; +} // namespace tinyxml2 namespace tesseract_common { @@ -363,7 +370,7 @@ void reorder(Eigen::Ref v, std::vector order); * @param value The value to update from the xml element * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE */ -tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLElement* xml_element, std::string& value); +int QueryStringValue(const tinyxml2::XMLElement* xml_element, std::string& value); /** * @brief Query a string Text from xml element @@ -371,7 +378,7 @@ tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLElement* xml_element, std * @param test The value to update from the xml element * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE */ -tinyxml2::XMLError QueryStringText(const tinyxml2::XMLElement* xml_element, std::string& text); +int QueryStringText(const tinyxml2::XMLElement* xml_element, std::string& text); /** * @brief Query a string value from xml attribute @@ -379,7 +386,7 @@ tinyxml2::XMLError QueryStringText(const tinyxml2::XMLElement* xml_element, std: * @param value The value to update from the xml attribute * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_WRONG_ATTRIBUTE_TYPE */ -tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, std::string& value); +int QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, std::string& value); /** * @brief Query a string attribute from an xml element @@ -389,7 +396,7 @@ tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or * tinyxml2::XML_WRONG_ATTRIBUTE_TYPE */ -tinyxml2::XMLError QueryStringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value); +int QueryStringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value); /** * @brief Get string attribute if exist. If it does not exist it returns the default value. @@ -412,9 +419,7 @@ std::string StringAttribute(const tinyxml2::XMLElement* xml_element, const char* * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or * tinyxml2::XML_WRONG_ATTRIBUTE_TYPE */ -tinyxml2::XMLError QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_element, - const char* name, - std::string& value); +int QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value); /** * @brief Query a double attribute from an xml element and print error log @@ -428,9 +433,7 @@ tinyxml2::XMLError QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_ * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or * tinyxml2::XML_WRONG_ATTRIBUTE_TYPE */ -tinyxml2::XMLError QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_element, - const char* name, - double& value); +int QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, double& value); /** * @brief Query a int attribute from an xml element and print error log @@ -444,7 +447,7 @@ tinyxml2::XMLError QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_ * @return tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or * tinyxml2::XML_WRONG_ATTRIBUTE_TYPE */ -tinyxml2::XMLError QueryIntAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, int& value); +int QueryIntAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, int& value); /** * @brief Check if two double are relatively equal diff --git a/tesseract_common/include/tesseract_common/yaml_utils.h b/tesseract_common/include/tesseract_common/yaml_utils.h index 895cc84291d..f1f626abd27 100644 --- a/tesseract_common/include/tesseract_common/yaml_utils.h +++ b/tesseract_common/include/tesseract_common/yaml_utils.h @@ -30,10 +30,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include +#include #include namespace tesseract_common diff --git a/tesseract_common/src/allowed_collision_matrix.cpp b/tesseract_common/src/allowed_collision_matrix.cpp index ea622689944..b1585b9f04e 100644 --- a/tesseract_common/src/allowed_collision_matrix.cpp +++ b/tesseract_common/src/allowed_collision_matrix.cpp @@ -57,6 +57,53 @@ bool operator==(const AllowedCollisionEntries& entries_1, const AllowedCollision return true; } +void AllowedCollisionMatrix::addAllowedCollision(const std::string& link_name1, + const std::string& link_name2, + const std::string& reason) +{ + auto link_pair = tesseract_common::makeOrderedLinkPair(link_name1, link_name2); + lookup_table_[link_pair] = reason; +} + +const AllowedCollisionEntries& AllowedCollisionMatrix::getAllAllowedCollisions() const { return lookup_table_; } + +void AllowedCollisionMatrix::removeAllowedCollision(const std::string& link_name1, const std::string& link_name2) +{ + auto link_pair = tesseract_common::makeOrderedLinkPair(link_name1, link_name2); + lookup_table_.erase(link_pair); +} + +void AllowedCollisionMatrix::removeAllowedCollision(const std::string& link_name) +{ + for (auto it = lookup_table_.begin(); it != lookup_table_.end() /* not hoisted */; /* no increment */) + { + if (it->first.first == link_name || it->first.second == link_name) + { + it = lookup_table_.erase(it); + } + else + { + ++it; + } + } +} + +bool AllowedCollisionMatrix::isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const +{ + thread_local LinkNamesPair link_pair; + tesseract_common::makeOrderedLinkPair(link_pair, link_name1, link_name2); + return (lookup_table_.find(link_pair) != lookup_table_.end()); +} + +void AllowedCollisionMatrix::clearAllowedCollisions() { lookup_table_.clear(); } + +void AllowedCollisionMatrix::insertAllowedCollisionMatrix(const AllowedCollisionMatrix& acm) +{ + lookup_table_.insert(acm.getAllAllowedCollisions().begin(), acm.getAllAllowedCollisions().end()); +} + +void AllowedCollisionMatrix::reserveAllowedCollisionMatrix(std::size_t size) { lookup_table_.reserve(size); } + bool AllowedCollisionMatrix::operator==(const AllowedCollisionMatrix& rhs) const { bool equal = true; @@ -71,6 +118,13 @@ void AllowedCollisionMatrix::serialize(Archive& ar, const unsigned int /*version { ar& BOOST_SERIALIZATION_NVP(lookup_table_); } + +std::ostream& operator<<(std::ostream& os, const AllowedCollisionMatrix& acm) +{ + for (const auto& pair : acm.getAllAllowedCollisions()) + os << "link=" << pair.first.first << " link=" << pair.first.second << " reason=" << pair.second << std::endl; + return os; +} } // namespace tesseract_common #include diff --git a/tesseract_common/src/calibration_info.cpp b/tesseract_common/src/calibration_info.cpp new file mode 100644 index 00000000000..8f3d0f1a5aa --- /dev/null +++ b/tesseract_common/src/calibration_info.cpp @@ -0,0 +1,72 @@ +/** + * @file calibration_info.cpp + * @brief Calibration Information + * + * @author Levi Armstrong + * @date January 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +#include +#include + +namespace tesseract_common +{ +void CalibrationInfo::insert(const CalibrationInfo& other) +{ + for (const auto& joint : other.joints) + joints[joint.first] = joint.second; +} + +void CalibrationInfo::clear() { joints.clear(); } + +bool CalibrationInfo::empty() const { return joints.empty(); } + +bool CalibrationInfo::operator==(const CalibrationInfo& rhs) const +{ + auto isometry_equal = [](const Eigen::Isometry3d& iso_1, const Eigen::Isometry3d& iso_2) { + return iso_1.isApprox(iso_2, 1e-5); + }; + + bool equal = true; + equal &= tesseract_common::isIdenticalMap(joints, rhs.joints, isometry_equal); + + return equal; +} +bool CalibrationInfo::operator!=(const CalibrationInfo& rhs) const { return !operator==(rhs); } + +template +void CalibrationInfo::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(joints); +} +} // namespace tesseract_common + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::CalibrationInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::CalibrationInfo) diff --git a/tesseract_common/src/collision_margin_data.cpp b/tesseract_common/src/collision_margin_data.cpp index 072889cec0f..63b9d384c57 100644 --- a/tesseract_common/src/collision_margin_data.cpp +++ b/tesseract_common/src/collision_margin_data.cpp @@ -39,6 +39,130 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_common { +CollisionMarginData::CollisionMarginData(double default_collision_margin) + : default_collision_margin_(default_collision_margin), max_collision_margin_(default_collision_margin) +{ +} + +CollisionMarginData::CollisionMarginData(double default_collision_margin, + PairsCollisionMarginData pair_collision_margins) + : default_collision_margin_(default_collision_margin), lookup_table_(std::move(pair_collision_margins)) +{ + updateMaxCollisionMargin(); +} + +CollisionMarginData::CollisionMarginData(PairsCollisionMarginData pair_collision_margins) + : lookup_table_(std::move(pair_collision_margins)) +{ + updateMaxCollisionMargin(); +} + +void CollisionMarginData::setDefaultCollisionMargin(double default_collision_margin) +{ + default_collision_margin_ = default_collision_margin; + updateMaxCollisionMargin(); +} + +double CollisionMarginData::getDefaultCollisionMargin() const { return default_collision_margin_; } + +void CollisionMarginData::setPairCollisionMargin(const std::string& obj1, + const std::string& obj2, + double collision_margin) +{ + auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2); + lookup_table_[key] = collision_margin; + updateMaxCollisionMargin(); +} + +double CollisionMarginData::getPairCollisionMargin(const std::string& obj1, const std::string& obj2) const +{ + thread_local LinkNamesPair key; + tesseract_common::makeOrderedLinkPair(key, obj1, obj2); + const auto it = lookup_table_.find(key); + + if (it != lookup_table_.end()) + return it->second; + + return default_collision_margin_; +} + +const PairsCollisionMarginData& CollisionMarginData::getPairCollisionMargins() const { return lookup_table_; } + +double CollisionMarginData::getMaxCollisionMargin() const { return max_collision_margin_; } + +void CollisionMarginData::incrementMargins(const double& increment) +{ + default_collision_margin_ += increment; + max_collision_margin_ += increment; + for (auto& pair : lookup_table_) + pair.second += increment; +} + +void CollisionMarginData::scaleMargins(const double& scale) +{ + default_collision_margin_ *= scale; + max_collision_margin_ *= scale; + for (auto& pair : lookup_table_) + pair.second *= scale; +} + +void CollisionMarginData::apply(const CollisionMarginData& collision_margin_data, + CollisionMarginOverrideType override_type) +{ + switch (override_type) + { + case CollisionMarginOverrideType::REPLACE: + { + *this = collision_margin_data; + break; + } + case CollisionMarginOverrideType::MODIFY: + { + default_collision_margin_ = collision_margin_data.default_collision_margin_; + + for (const auto& p : collision_margin_data.lookup_table_) + lookup_table_[p.first] = p.second; + + updateMaxCollisionMargin(); + break; + } + case CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN: + { + default_collision_margin_ = collision_margin_data.default_collision_margin_; + updateMaxCollisionMargin(); + break; + } + case CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN: + { + lookup_table_ = collision_margin_data.lookup_table_; + updateMaxCollisionMargin(); + break; + } + case CollisionMarginOverrideType::MODIFY_PAIR_MARGIN: + { + for (const auto& p : collision_margin_data.lookup_table_) + lookup_table_[p.first] = p.second; + + updateMaxCollisionMargin(); + break; + } + case CollisionMarginOverrideType::NONE: + { + break; + } + } +} + +void CollisionMarginData::updateMaxCollisionMargin() +{ + max_collision_margin_ = default_collision_margin_; + for (const auto& p : lookup_table_) + { + if (p.second > max_collision_margin_) + max_collision_margin_ = p.second; + } +} + bool CollisionMarginData::operator==(const CollisionMarginData& rhs) const { bool ret_val = true; diff --git a/tesseract_common/src/plugin_info.cpp b/tesseract_common/src/plugin_info.cpp new file mode 100644 index 00000000000..fa7a7a3ae07 --- /dev/null +++ b/tesseract_common/src/plugin_info.cpp @@ -0,0 +1,303 @@ +/** + * @file plugin_info.cpp + * @brief Common Tesseract Plugin Infos + * + * @author Levi Armstrong + * @date January 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +namespace tesseract_common +{ +/*********************************************************/ +/****** PluginInfo *****/ +/*********************************************************/ + +std::string PluginInfo::getConfigString() const { return toYAMLString(config); } + +bool PluginInfo::operator==(const PluginInfo& rhs) const +{ + bool equal = true; + equal &= class_name == rhs.class_name; + equal &= compareYAML(config, rhs.config); + + return equal; +} +bool PluginInfo::operator!=(const PluginInfo& rhs) const { return !operator==(rhs); } + +template +void PluginInfo::save(Archive& ar, const unsigned int /*version*/) const +{ + ar& BOOST_SERIALIZATION_NVP(class_name); + std::string config_string = getConfigString(); + ar& BOOST_SERIALIZATION_NVP(config_string); +} + +template +void PluginInfo::load(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(class_name); + std::string config_string; + ar& BOOST_SERIALIZATION_NVP(config_string); + // On 18.04 '~' does not load as null so must check + config = (config_string != "~") ? YAML::Load(config_string) : YAML::Node(); +} + +template +void PluginInfo::serialize(Archive& ar, const unsigned int version) +{ + boost::serialization::split_member(ar, *this, version); +} + +/*********************************************************/ +/****** PluginInfoContainer *****/ +/*********************************************************/ + +void PluginInfoContainer::clear() +{ + default_plugin.clear(); + plugins.clear(); +} + +bool PluginInfoContainer::operator==(const PluginInfoContainer& rhs) const +{ + bool equal = true; + equal &= default_plugin == rhs.default_plugin; + equal &= isIdenticalMap(plugins, rhs.plugins); + + return equal; +} +bool PluginInfoContainer::operator!=(const PluginInfoContainer& rhs) const { return !operator==(rhs); } + +template +void PluginInfoContainer::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(default_plugin); + ar& BOOST_SERIALIZATION_NVP(plugins); +} + +/*********************************************************/ +/****** KinematicsPluginInfo *****/ +/*********************************************************/ +void KinematicsPluginInfo::insert(const KinematicsPluginInfo& other) +{ + search_paths.insert(other.search_paths.begin(), other.search_paths.end()); + search_libraries.insert(other.search_libraries.begin(), other.search_libraries.end()); + + for (const auto& group_plugins : other.fwd_plugin_infos) + { + if (!group_plugins.second.default_plugin.empty()) + fwd_plugin_infos[group_plugins.first].default_plugin = group_plugins.second.default_plugin; + + for (const auto& plugin_info : group_plugins.second.plugins) + fwd_plugin_infos[group_plugins.first].plugins[plugin_info.first] = plugin_info.second; + } + + for (const auto& group_plugins : other.inv_plugin_infos) + { + if (!group_plugins.second.default_plugin.empty()) + inv_plugin_infos[group_plugins.first].default_plugin = group_plugins.second.default_plugin; + + for (const auto& plugin_info : group_plugins.second.plugins) + inv_plugin_infos[group_plugins.first].plugins[plugin_info.first] = plugin_info.second; + } +} + +void KinematicsPluginInfo::clear() +{ + search_paths.clear(); + search_libraries.clear(); + fwd_plugin_infos.clear(); + inv_plugin_infos.clear(); +} + +bool KinematicsPluginInfo::empty() const +{ + return (search_paths.empty() && search_libraries.empty() && fwd_plugin_infos.empty() && inv_plugin_infos.empty()); +} + +bool KinematicsPluginInfo::operator==(const KinematicsPluginInfo& rhs) const +{ + bool equal = true; + equal &= isIdenticalSet(search_paths, rhs.search_paths); + equal &= isIdenticalSet(search_libraries, rhs.search_libraries); + equal &= isIdenticalMap, PluginInfoContainer>(fwd_plugin_infos, + rhs.fwd_plugin_infos); + equal &= isIdenticalMap, PluginInfoContainer>(inv_plugin_infos, + rhs.inv_plugin_infos); + + return equal; +} +bool KinematicsPluginInfo::operator!=(const KinematicsPluginInfo& rhs) const { return !operator==(rhs); } + +template +void KinematicsPluginInfo::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(search_paths); + ar& BOOST_SERIALIZATION_NVP(search_libraries); + ar& BOOST_SERIALIZATION_NVP(fwd_plugin_infos); + ar& BOOST_SERIALIZATION_NVP(inv_plugin_infos); +} + +/*********************************************************/ +/****** ContactManagersPluginInfo *****/ +/*********************************************************/ +void ContactManagersPluginInfo::insert(const ContactManagersPluginInfo& other) +{ + search_paths.insert(other.search_paths.begin(), other.search_paths.end()); + search_libraries.insert(other.search_libraries.begin(), other.search_libraries.end()); + + if (!other.discrete_plugin_infos.default_plugin.empty()) + discrete_plugin_infos.default_plugin = other.discrete_plugin_infos.default_plugin; + + for (const auto& plugin_info : other.discrete_plugin_infos.plugins) + discrete_plugin_infos.plugins[plugin_info.first] = plugin_info.second; + + if (!other.continuous_plugin_infos.default_plugin.empty()) + continuous_plugin_infos.default_plugin = other.continuous_plugin_infos.default_plugin; + + for (const auto& plugin_info : other.continuous_plugin_infos.plugins) + continuous_plugin_infos.plugins[plugin_info.first] = plugin_info.second; + + if (!other.discrete_plugin_infos.default_plugin.empty()) + discrete_plugin_infos.default_plugin = other.discrete_plugin_infos.default_plugin; + + if (!other.continuous_plugin_infos.default_plugin.empty()) + continuous_plugin_infos.default_plugin = other.continuous_plugin_infos.default_plugin; +} + +void ContactManagersPluginInfo::clear() +{ + search_paths.clear(); + search_libraries.clear(); + discrete_plugin_infos.clear(); + continuous_plugin_infos.clear(); +} + +bool ContactManagersPluginInfo::empty() const +{ + return (search_paths.empty() && search_libraries.empty() && discrete_plugin_infos.plugins.empty() && + continuous_plugin_infos.plugins.empty()); +} + +bool ContactManagersPluginInfo::operator==(const ContactManagersPluginInfo& rhs) const +{ + bool equal = true; + equal &= isIdenticalSet(search_paths, rhs.search_paths); + equal &= isIdenticalSet(search_libraries, rhs.search_libraries); + equal &= (discrete_plugin_infos == rhs.discrete_plugin_infos); + equal &= (continuous_plugin_infos == rhs.continuous_plugin_infos); + return equal; +} +bool ContactManagersPluginInfo::operator!=(const ContactManagersPluginInfo& rhs) const { return !operator==(rhs); } + +template +void ContactManagersPluginInfo::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(search_paths); + ar& BOOST_SERIALIZATION_NVP(search_libraries); + ar& BOOST_SERIALIZATION_NVP(discrete_plugin_infos); + ar& BOOST_SERIALIZATION_NVP(continuous_plugin_infos); +} + +/*********************************************************/ +/****** TaskComposerPluginInfo *****/ +/*********************************************************/ +void TaskComposerPluginInfo::insert(const TaskComposerPluginInfo& other) +{ + search_paths.insert(other.search_paths.begin(), other.search_paths.end()); + search_libraries.insert(other.search_libraries.begin(), other.search_libraries.end()); + + if (!other.executor_plugin_infos.default_plugin.empty()) + executor_plugin_infos.default_plugin = other.executor_plugin_infos.default_plugin; + + for (const auto& plugin_info : other.executor_plugin_infos.plugins) + executor_plugin_infos.plugins[plugin_info.first] = plugin_info.second; + + if (!other.task_plugin_infos.default_plugin.empty()) + task_plugin_infos.default_plugin = other.task_plugin_infos.default_plugin; + + for (const auto& plugin_info : other.task_plugin_infos.plugins) + task_plugin_infos.plugins[plugin_info.first] = plugin_info.second; +} + +void TaskComposerPluginInfo::clear() +{ + search_paths.clear(); + search_libraries.clear(); + executor_plugin_infos.clear(); + task_plugin_infos.clear(); +} + +bool TaskComposerPluginInfo::empty() const +{ + return (search_paths.empty() && search_libraries.empty() && executor_plugin_infos.plugins.empty() && + task_plugin_infos.plugins.empty()); +} + +bool TaskComposerPluginInfo::operator==(const TaskComposerPluginInfo& rhs) const +{ + bool equal = true; + equal &= isIdenticalSet(search_paths, rhs.search_paths); + equal &= isIdenticalSet(search_libraries, rhs.search_libraries); + equal &= (executor_plugin_infos == rhs.executor_plugin_infos); + equal &= (task_plugin_infos == rhs.task_plugin_infos); + return equal; +} +bool TaskComposerPluginInfo::operator!=(const TaskComposerPluginInfo& rhs) const { return !operator==(rhs); } + +template +void TaskComposerPluginInfo::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(search_paths); + ar& BOOST_SERIALIZATION_NVP(search_libraries); + ar& BOOST_SERIALIZATION_NVP(executor_plugin_infos); + ar& BOOST_SERIALIZATION_NVP(task_plugin_infos); +} + +} // namespace tesseract_common + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::PluginInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::PluginInfo) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::PluginInfoContainer) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::PluginInfoContainer) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::KinematicsPluginInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::KinematicsPluginInfo) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ContactManagersPluginInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ContactManagersPluginInfo) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::TaskComposerPluginInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::TaskComposerPluginInfo) diff --git a/tesseract_common/src/timer.cpp b/tesseract_common/src/timer.cpp new file mode 100644 index 00000000000..3d1280b1ef0 --- /dev/null +++ b/tesseract_common/src/timer.cpp @@ -0,0 +1,53 @@ +/** + * @file timer.cpp + * @brief Simple timer class using chrono + * + * @author Levi Armstrong + * @date February 2, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +namespace tesseract_common +{ +void Timer::start() +{ + start_time_ = Clock::now(); + running_ = true; +} + +void Timer::stop() +{ + end_time_ = Clock::now(); + running_ = false; +} + +double Timer::elapsedMilliseconds() const +{ + if (running_) + return std::chrono::duration(Clock::now() - start_time_).count(); + + return std::chrono::duration(end_time_ - start_time_).count(); +} + +double Timer::elapsedSeconds() const { return (elapsedMilliseconds() / 1000.0); } + +} // namespace tesseract_common diff --git a/tesseract_common/src/types.cpp b/tesseract_common/src/types.cpp index a74b0de8852..2cafdc2289d 100644 --- a/tesseract_common/src/types.cpp +++ b/tesseract_common/src/types.cpp @@ -24,22 +24,7 @@ * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include #include -#include namespace tesseract_common { @@ -70,296 +55,4 @@ void makeOrderedLinkPair(LinkNamesPair& pair, const std::string& link_name1, con pair.second = link_name1; } } - -/*********************************************************/ -/****** PluginInfo *****/ -/*********************************************************/ - -std::string PluginInfo::getConfigString() const { return toYAMLString(config); } - -bool PluginInfo::operator==(const PluginInfo& rhs) const -{ - bool equal = true; - equal &= class_name == rhs.class_name; - equal &= compareYAML(config, rhs.config); - - return equal; -} -bool PluginInfo::operator!=(const PluginInfo& rhs) const { return !operator==(rhs); } - -template -void PluginInfo::save(Archive& ar, const unsigned int /*version*/) const -{ - ar& BOOST_SERIALIZATION_NVP(class_name); - std::string config_string = getConfigString(); - ar& BOOST_SERIALIZATION_NVP(config_string); -} - -template -void PluginInfo::load(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_NVP(class_name); - std::string config_string; - ar& BOOST_SERIALIZATION_NVP(config_string); - // On 18.04 '~' does not load as null so must check - config = (config_string != "~") ? YAML::Load(config_string) : YAML::Node(); -} - -template -void PluginInfo::serialize(Archive& ar, const unsigned int version) -{ - boost::serialization::split_member(ar, *this, version); -} - -/*********************************************************/ -/****** PluginInfoContainer *****/ -/*********************************************************/ - -void PluginInfoContainer::clear() -{ - default_plugin.clear(); - plugins.clear(); -} - -bool PluginInfoContainer::operator==(const PluginInfoContainer& rhs) const -{ - bool equal = true; - equal &= default_plugin == rhs.default_plugin; - equal &= isIdenticalMap(plugins, rhs.plugins); - - return equal; -} -bool PluginInfoContainer::operator!=(const PluginInfoContainer& rhs) const { return !operator==(rhs); } - -template -void PluginInfoContainer::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_NVP(default_plugin); - ar& BOOST_SERIALIZATION_NVP(plugins); -} - -/*********************************************************/ -/****** KinematicsPluginInfo *****/ -/*********************************************************/ -void KinematicsPluginInfo::insert(const KinematicsPluginInfo& other) -{ - search_paths.insert(other.search_paths.begin(), other.search_paths.end()); - search_libraries.insert(other.search_libraries.begin(), other.search_libraries.end()); - - for (const auto& group_plugins : other.fwd_plugin_infos) - { - if (!group_plugins.second.default_plugin.empty()) - fwd_plugin_infos[group_plugins.first].default_plugin = group_plugins.second.default_plugin; - - for (const auto& plugin_info : group_plugins.second.plugins) - fwd_plugin_infos[group_plugins.first].plugins[plugin_info.first] = plugin_info.second; - } - - for (const auto& group_plugins : other.inv_plugin_infos) - { - if (!group_plugins.second.default_plugin.empty()) - inv_plugin_infos[group_plugins.first].default_plugin = group_plugins.second.default_plugin; - - for (const auto& plugin_info : group_plugins.second.plugins) - inv_plugin_infos[group_plugins.first].plugins[plugin_info.first] = plugin_info.second; - } -} - -void KinematicsPluginInfo::clear() -{ - search_paths.clear(); - search_libraries.clear(); - fwd_plugin_infos.clear(); - inv_plugin_infos.clear(); -} - -bool KinematicsPluginInfo::empty() const -{ - return (search_paths.empty() && search_libraries.empty() && fwd_plugin_infos.empty() && inv_plugin_infos.empty()); -} - -bool KinematicsPluginInfo::operator==(const KinematicsPluginInfo& rhs) const -{ - bool equal = true; - equal &= isIdenticalSet(search_paths, rhs.search_paths); - equal &= isIdenticalSet(search_libraries, rhs.search_libraries); - equal &= isIdenticalMap, PluginInfoContainer>(fwd_plugin_infos, - rhs.fwd_plugin_infos); - equal &= isIdenticalMap, PluginInfoContainer>(inv_plugin_infos, - rhs.inv_plugin_infos); - - return equal; -} -bool KinematicsPluginInfo::operator!=(const KinematicsPluginInfo& rhs) const { return !operator==(rhs); } - -template -void KinematicsPluginInfo::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_NVP(search_paths); - ar& BOOST_SERIALIZATION_NVP(search_libraries); - ar& BOOST_SERIALIZATION_NVP(fwd_plugin_infos); - ar& BOOST_SERIALIZATION_NVP(inv_plugin_infos); -} - -/*********************************************************/ -/****** ContactManagersPluginInfo *****/ -/*********************************************************/ -void ContactManagersPluginInfo::insert(const ContactManagersPluginInfo& other) -{ - search_paths.insert(other.search_paths.begin(), other.search_paths.end()); - search_libraries.insert(other.search_libraries.begin(), other.search_libraries.end()); - - if (!other.discrete_plugin_infos.default_plugin.empty()) - discrete_plugin_infos.default_plugin = other.discrete_plugin_infos.default_plugin; - - for (const auto& plugin_info : other.discrete_plugin_infos.plugins) - discrete_plugin_infos.plugins[plugin_info.first] = plugin_info.second; - - if (!other.continuous_plugin_infos.default_plugin.empty()) - continuous_plugin_infos.default_plugin = other.continuous_plugin_infos.default_plugin; - - for (const auto& plugin_info : other.continuous_plugin_infos.plugins) - continuous_plugin_infos.plugins[plugin_info.first] = plugin_info.second; - - if (!other.discrete_plugin_infos.default_plugin.empty()) - discrete_plugin_infos.default_plugin = other.discrete_plugin_infos.default_plugin; - - if (!other.continuous_plugin_infos.default_plugin.empty()) - continuous_plugin_infos.default_plugin = other.continuous_plugin_infos.default_plugin; -} - -void ContactManagersPluginInfo::clear() -{ - search_paths.clear(); - search_libraries.clear(); - discrete_plugin_infos.clear(); - continuous_plugin_infos.clear(); -} - -bool ContactManagersPluginInfo::empty() const -{ - return (search_paths.empty() && search_libraries.empty() && discrete_plugin_infos.plugins.empty() && - continuous_plugin_infos.plugins.empty()); -} - -bool ContactManagersPluginInfo::operator==(const ContactManagersPluginInfo& rhs) const -{ - bool equal = true; - equal &= isIdenticalSet(search_paths, rhs.search_paths); - equal &= isIdenticalSet(search_libraries, rhs.search_libraries); - equal &= (discrete_plugin_infos == rhs.discrete_plugin_infos); - equal &= (continuous_plugin_infos == rhs.continuous_plugin_infos); - return equal; -} -bool ContactManagersPluginInfo::operator!=(const ContactManagersPluginInfo& rhs) const { return !operator==(rhs); } - -template -void ContactManagersPluginInfo::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_NVP(search_paths); - ar& BOOST_SERIALIZATION_NVP(search_libraries); - ar& BOOST_SERIALIZATION_NVP(discrete_plugin_infos); - ar& BOOST_SERIALIZATION_NVP(continuous_plugin_infos); -} - -/*********************************************************/ -/****** TaskComposerPluginInfo *****/ -/*********************************************************/ -void TaskComposerPluginInfo::insert(const TaskComposerPluginInfo& other) -{ - search_paths.insert(other.search_paths.begin(), other.search_paths.end()); - search_libraries.insert(other.search_libraries.begin(), other.search_libraries.end()); - - if (!other.executor_plugin_infos.default_plugin.empty()) - executor_plugin_infos.default_plugin = other.executor_plugin_infos.default_plugin; - - for (const auto& plugin_info : other.executor_plugin_infos.plugins) - executor_plugin_infos.plugins[plugin_info.first] = plugin_info.second; - - if (!other.task_plugin_infos.default_plugin.empty()) - task_plugin_infos.default_plugin = other.task_plugin_infos.default_plugin; - - for (const auto& plugin_info : other.task_plugin_infos.plugins) - task_plugin_infos.plugins[plugin_info.first] = plugin_info.second; -} - -void TaskComposerPluginInfo::clear() -{ - search_paths.clear(); - search_libraries.clear(); - executor_plugin_infos.clear(); - task_plugin_infos.clear(); -} - -bool TaskComposerPluginInfo::empty() const -{ - return (search_paths.empty() && search_libraries.empty() && executor_plugin_infos.plugins.empty() && - task_plugin_infos.plugins.empty()); -} - -bool TaskComposerPluginInfo::operator==(const TaskComposerPluginInfo& rhs) const -{ - bool equal = true; - equal &= isIdenticalSet(search_paths, rhs.search_paths); - equal &= isIdenticalSet(search_libraries, rhs.search_libraries); - equal &= (executor_plugin_infos == rhs.executor_plugin_infos); - equal &= (task_plugin_infos == rhs.task_plugin_infos); - return equal; -} -bool TaskComposerPluginInfo::operator!=(const TaskComposerPluginInfo& rhs) const { return !operator==(rhs); } - -template -void TaskComposerPluginInfo::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_NVP(search_paths); - ar& BOOST_SERIALIZATION_NVP(search_libraries); - ar& BOOST_SERIALIZATION_NVP(executor_plugin_infos); - ar& BOOST_SERIALIZATION_NVP(task_plugin_infos); -} - -/*********************************************************/ -/****** CalibrationInfo *****/ -/*********************************************************/ -void CalibrationInfo::insert(const CalibrationInfo& other) -{ - for (const auto& joint : other.joints) - joints[joint.first] = joint.second; -} - -void CalibrationInfo::clear() { joints.clear(); } - -bool CalibrationInfo::empty() const { return joints.empty(); } - -bool CalibrationInfo::operator==(const CalibrationInfo& rhs) const -{ - auto isometry_equal = [](const Eigen::Isometry3d& iso_1, const Eigen::Isometry3d& iso_2) { - return iso_1.isApprox(iso_2, 1e-5); - }; - - bool equal = true; - equal &= tesseract_common::isIdenticalMap(joints, rhs.joints, isometry_equal); - - return equal; -} -bool CalibrationInfo::operator!=(const CalibrationInfo& rhs) const { return !operator==(rhs); } - -template -void CalibrationInfo::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_NVP(joints); -} } // namespace tesseract_common - -#include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::PluginInfo) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::PluginInfo) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::PluginInfoContainer) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::PluginInfoContainer) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::KinematicsPluginInfo) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::KinematicsPluginInfo) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ContactManagersPluginInfo) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ContactManagersPluginInfo) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::TaskComposerPluginInfo) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::TaskComposerPluginInfo) -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::CalibrationInfo) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::CalibrationInfo) diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp index dd6293461e8..b1cc3abd154 100644 --- a/tesseract_common/src/utils.cpp +++ b/tesseract_common/src/utils.cpp @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -331,7 +332,7 @@ void reorder(Eigen::Ref v, std::vector order) } } -tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLElement* xml_element, std::string& value) +int QueryStringValue(const tinyxml2::XMLElement* xml_element, std::string& value) { if (xml_element->Value() == nullptr) return tinyxml2::XML_NO_ATTRIBUTE; @@ -341,7 +342,7 @@ tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLElement* xml_element, std return tinyxml2::XML_SUCCESS; } -tinyxml2::XMLError QueryStringText(const tinyxml2::XMLElement* xml_element, std::string& text) +int QueryStringText(const tinyxml2::XMLElement* xml_element, std::string& text) { if (xml_element->GetText() == nullptr) return tinyxml2::XML_NO_ATTRIBUTE; @@ -351,7 +352,7 @@ tinyxml2::XMLError QueryStringText(const tinyxml2::XMLElement* xml_element, std: return tinyxml2::XML_SUCCESS; } -tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, std::string& value) +int QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, std::string& value) { if (xml_attribute->Value() == nullptr) return tinyxml2::XML_WRONG_ATTRIBUTE_TYPE; @@ -361,7 +362,7 @@ tinyxml2::XMLError QueryStringValue(const tinyxml2::XMLAttribute* xml_attribute, return tinyxml2::XML_SUCCESS; } -tinyxml2::XMLError QueryStringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value) +int QueryStringAttribute(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value) { const tinyxml2::XMLAttribute* attribute = xml_element->FindAttribute(name); if (attribute == nullptr) @@ -377,11 +378,9 @@ std::string StringAttribute(const tinyxml2::XMLElement* xml_element, const char* return str; } -tinyxml2::XMLError QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_element, - const char* name, - std::string& value) +int QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, std::string& value) { - tinyxml2::XMLError status = QueryStringAttribute(xml_element, name, value); + int status = QueryStringAttribute(xml_element, name, value); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) { @@ -399,11 +398,9 @@ tinyxml2::XMLError QueryStringAttributeRequired(const tinyxml2::XMLElement* xml_ return status; } -tinyxml2::XMLError QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_element, - const char* name, - double& value) +int QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, double& value) { - tinyxml2::XMLError status = xml_element->QueryDoubleAttribute(name, &value); + int status = xml_element->QueryDoubleAttribute(name, &value); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) { @@ -421,9 +418,9 @@ tinyxml2::XMLError QueryDoubleAttributeRequired(const tinyxml2::XMLElement* xml_ return status; } -tinyxml2::XMLError QueryIntAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, int& value) +int QueryIntAttributeRequired(const tinyxml2::XMLElement* xml_element, const char* name, int& value) { - tinyxml2::XMLError status = xml_element->QueryIntAttribute(name, &value); + int status = xml_element->QueryIntAttribute(name, &value); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) { diff --git a/tesseract_common/test/tesseract_common_serialization_unit.cpp b/tesseract_common/test/tesseract_common_serialization_unit.cpp index 4a5ad2cce61..fc1f9c3065a 100644 --- a/tesseract_common/test/tesseract_common_serialization_unit.cpp +++ b/tesseract_common/test/tesseract_common_serialization_unit.cpp @@ -39,6 +39,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include using namespace tesseract_common; diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp index c867c29cf1d..c3b25f244ad 100644 --- a/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract_common/test/tesseract_common_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -655,7 +656,7 @@ TEST(TesseractCommonUnit, QueryStringValueUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringValue(element, string_value); + int status = tesseract_common::QueryStringValue(element, string_value); EXPECT_TRUE(status == tinyxml2::XML_SUCCESS); EXPECT_TRUE(string_value == "box"); } @@ -672,7 +673,7 @@ TEST(TesseractCommonUnit, QueryStringTextUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringText(element, string_value); + int status = tesseract_common::QueryStringText(element, string_value); EXPECT_TRUE(status == tinyxml2::XML_SUCCESS); EXPECT_TRUE(string_value == "Test"); } @@ -686,7 +687,7 @@ TEST(TesseractCommonUnit, QueryStringTextUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringText(element, string_value); + int status = tesseract_common::QueryStringText(element, string_value); EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE); } } @@ -702,7 +703,7 @@ TEST(TesseractCommonUnit, QueryStringAttributeUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringAttribute(element, "name", string_value); + int status = tesseract_common::QueryStringAttribute(element, "name", string_value); EXPECT_TRUE(status == tinyxml2::XML_SUCCESS); EXPECT_TRUE(string_value == "test"); } @@ -716,7 +717,7 @@ TEST(TesseractCommonUnit, QueryStringAttributeUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringAttribute(element, "name", string_value); + int status = tesseract_common::QueryStringAttribute(element, "name", string_value); EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE); } } @@ -759,7 +760,7 @@ TEST(TesseractCommonUnit, QueryStringAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(element, "name", string_value); + int status = tesseract_common::QueryStringAttributeRequired(element, "name", string_value); EXPECT_TRUE(status == tinyxml2::XML_SUCCESS); EXPECT_TRUE(string_value == "test"); } @@ -773,7 +774,7 @@ TEST(TesseractCommonUnit, QueryStringAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); std::string string_value; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(element, "missing", string_value); + int status = tesseract_common::QueryStringAttributeRequired(element, "missing", string_value); EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE); } } @@ -789,7 +790,7 @@ TEST(TesseractCommonUnit, QueryDoubleAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); double double_value{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryDoubleAttributeRequired(element, "name", double_value); + int status = tesseract_common::QueryDoubleAttributeRequired(element, "name", double_value); EXPECT_TRUE(status == tinyxml2::XML_SUCCESS); EXPECT_NEAR(double_value, 1.5, 1e-6); } @@ -803,7 +804,7 @@ TEST(TesseractCommonUnit, QueryDoubleAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); double double_value{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryDoubleAttributeRequired(element, "missing", double_value); + int status = tesseract_common::QueryDoubleAttributeRequired(element, "missing", double_value); EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE); } @@ -816,7 +817,7 @@ TEST(TesseractCommonUnit, QueryDoubleAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); double double_value{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryDoubleAttributeRequired(element, "name", double_value); + int status = tesseract_common::QueryDoubleAttributeRequired(element, "name", double_value); EXPECT_TRUE(status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE); } } @@ -832,7 +833,7 @@ TEST(TesseractCommonUnit, QueryIntAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); int int_value{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryIntAttributeRequired(element, "name", int_value); + int status = tesseract_common::QueryIntAttributeRequired(element, "name", int_value); EXPECT_TRUE(status == tinyxml2::XML_SUCCESS); EXPECT_NEAR(int_value, 1, 1e-6); } @@ -846,7 +847,7 @@ TEST(TesseractCommonUnit, QueryIntAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); int int_value{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryIntAttributeRequired(element, "missing", int_value); + int status = tesseract_common::QueryIntAttributeRequired(element, "missing", int_value); EXPECT_TRUE(status == tinyxml2::XML_NO_ATTRIBUTE); } @@ -859,7 +860,7 @@ TEST(TesseractCommonUnit, QueryIntAttributeRequiredUnit) // NOLINT EXPECT_TRUE(element != nullptr); int int_value{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryIntAttributeRequired(element, "name", int_value); + int status = tesseract_common::QueryIntAttributeRequired(element, "name", int_value); EXPECT_TRUE(status == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE); } } diff --git a/tesseract_environment/CMakeLists.txt b/tesseract_environment/CMakeLists.txt index 77498416ab5..1642c2b153b 100644 --- a/tesseract_environment/CMakeLists.txt +++ b/tesseract_environment/CMakeLists.txt @@ -47,7 +47,12 @@ set(COVERAGE_EXCLUDE add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) # Create interface for core -add_library(${PROJECT_NAME} src/environment.cpp src/environment_cache.cpp src/utils.cpp) +add_library(${PROJECT_NAME} + src/environment.cpp + src/environment_cache.cpp + src/environment_monitor.cpp + src/events.cpp + src/utils.cpp) target_link_libraries( ${PROJECT_NAME} PUBLIC Eigen3::Eigen diff --git a/tesseract_environment/include/tesseract_environment/command.h b/tesseract_environment/include/tesseract_environment/command.h index eac40e556d6..e6fbc755593 100644 --- a/tesseract_environment/include/tesseract_environment/command.h +++ b/tesseract_environment/include/tesseract_environment/command.h @@ -99,7 +99,7 @@ class Command void serialize(Archive& ar, const unsigned int version); // NOLINT }; -using Commands = std::vector; +using Commands = std::vector>; } // namespace tesseract_environment #endif // COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/add_contact_managers_plugin_info_command.h b/tesseract_environment/include/tesseract_environment/commands/add_contact_managers_plugin_info_command.h index 72b79301ec5..9d96bf6c2ee 100644 --- a/tesseract_environment/include/tesseract_environment/commands/add_contact_managers_plugin_info_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/add_contact_managers_plugin_info_command.h @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include namespace tesseract_environment { @@ -64,7 +64,6 @@ class AddContactManagersPluginInfoCommand : public Command }; } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::AddContactManagersPluginInfoCommand, "AddContactManagersPluginInfoCommand") diff --git a/tesseract_environment/include/tesseract_environment/commands/add_kinematics_information_command.h b/tesseract_environment/include/tesseract_environment/commands/add_kinematics_information_command.h index 184b5ae0dcb..2b1c898a025 100644 --- a/tesseract_environment/include/tesseract_environment/commands/add_kinematics_information_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/add_kinematics_information_command.h @@ -65,6 +65,5 @@ class AddKinematicsInformationCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::AddKinematicsInformationCommand, "AddKinematicsInformationCommand") #endif // TESSERACT_ENVIRONMENT_ADD_KINEMATICS_INFORMATION_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/add_link_command.h b/tesseract_environment/include/tesseract_environment/commands/add_link_command.h index a7b33123baf..b36589515ca 100644 --- a/tesseract_environment/include/tesseract_environment/commands/add_link_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/add_link_command.h @@ -33,8 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include -#include +#include namespace tesseract_environment { @@ -90,16 +89,16 @@ class AddLinkCommand : public Command const tesseract_scene_graph::Joint& joint, bool replace_allowed = false); - const tesseract_scene_graph::Link::ConstPtr& getLink() const; - const tesseract_scene_graph::Joint::ConstPtr& getJoint() const; + const std::shared_ptr& getLink() const; + const std::shared_ptr& getJoint() const; bool replaceAllowed() const; bool operator==(const AddLinkCommand& rhs) const; bool operator!=(const AddLinkCommand& rhs) const; private: - tesseract_scene_graph::Link::ConstPtr link_; - tesseract_scene_graph::Joint::ConstPtr joint_; + std::shared_ptr link_; + std::shared_ptr joint_; bool replace_allowed_{ false }; friend class boost::serialization::access; @@ -109,7 +108,6 @@ class AddLinkCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::AddLinkCommand, "AddLinkCommand") #endif // TESSERACT_ENVIRONMENT_ADD_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/add_scene_graph_command.h b/tesseract_environment/include/tesseract_environment/commands/add_scene_graph_command.h index 26c9bff0213..ccb1609e78b 100644 --- a/tesseract_environment/include/tesseract_environment/commands/add_scene_graph_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/add_scene_graph_command.h @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include namespace tesseract_environment { @@ -70,16 +70,16 @@ class AddSceneGraphCommand : public Command const tesseract_scene_graph::Joint& joint, std::string prefix = ""); - const tesseract_scene_graph::SceneGraph::ConstPtr& getSceneGraph() const; - const tesseract_scene_graph::Joint::ConstPtr& getJoint() const; + const std::shared_ptr& getSceneGraph() const; + const std::shared_ptr& getJoint() const; const std::string& getPrefix() const; bool operator==(const AddSceneGraphCommand& rhs) const; bool operator!=(const AddSceneGraphCommand& rhs) const; private: - tesseract_scene_graph::SceneGraph::ConstPtr scene_graph_; - tesseract_scene_graph::Joint::ConstPtr joint_; + std::shared_ptr scene_graph_; + std::shared_ptr joint_; std::string prefix_; friend class boost::serialization::access; @@ -88,7 +88,6 @@ class AddSceneGraphCommand : public Command }; } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::AddSceneGraphCommand, "AddSceneGraphCommand") #endif // TESSERACT_ENVIRONMENT_ADD_SCENE_GRAPH_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/add_trajectory_link_command.h b/tesseract_environment/include/tesseract_environment/commands/add_trajectory_link_command.h index 69fd83c1e72..b42675a3b5b 100644 --- a/tesseract_environment/include/tesseract_environment/commands/add_trajectory_link_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/add_trajectory_link_command.h @@ -33,7 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include #include namespace tesseract_environment @@ -92,7 +91,6 @@ class AddTrajectoryLinkCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::AddTrajectoryLinkCommand, "AddTrajectoryLinkCommand") #endif // TESSERACT_ENVIRONMENT_ADD_TRAJECTORY_LINK_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_collision_margins_command.h b/tesseract_environment/include/tesseract_environment/commands/change_collision_margins_command.h index a27220105e3..bd6a96410b0 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_collision_margins_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_collision_margins_command.h @@ -77,6 +77,5 @@ class ChangeCollisionMarginsCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeCollisionMarginsCommand, "ChangeCollisionMarginsCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_COLLISION_MARGINS_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_joint_acceleration_limits_command.h b/tesseract_environment/include/tesseract_environment/commands/change_joint_acceleration_limits_command.h index 5faee0c443e..65e55048149 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_joint_acceleration_limits_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_joint_acceleration_limits_command.h @@ -73,7 +73,6 @@ class ChangeJointAccelerationLimitsCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeJointAccelerationLimitsCommand, "ChangeJointAccelerationLimitsCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_JOINT_ACCELERATION_LIMITS_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_joint_origin_command.h b/tesseract_environment/include/tesseract_environment/commands/change_joint_origin_command.h index 81e39bba54f..59428d32476 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_joint_origin_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_joint_origin_command.h @@ -76,6 +76,5 @@ class ChangeJointOriginCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeJointOriginCommand, "ChangeJointOriginCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_JOINT_ORIGIN_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_joint_position_limits_command.h b/tesseract_environment/include/tesseract_environment/commands/change_joint_position_limits_command.h index aac0ff79ca1..a6099ba4e9d 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_joint_position_limits_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_joint_position_limits_command.h @@ -74,6 +74,5 @@ class ChangeJointPositionLimitsCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeJointPositionLimitsCommand, "ChangeJointPositionLimitsCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_JOINT_POSITION_LIMITS_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_joint_velocity_limits_command.h b/tesseract_environment/include/tesseract_environment/commands/change_joint_velocity_limits_command.h index 38dea935423..cdd2eaadc3f 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_joint_velocity_limits_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_joint_velocity_limits_command.h @@ -73,6 +73,5 @@ class ChangeJointVelocityLimitsCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeJointVelocityLimitsCommand, "ChangeJointVelocityLimitsCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_JOINT_VELOCITY_LIMITS_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_link_collision_enabled_command.h b/tesseract_environment/include/tesseract_environment/commands/change_link_collision_enabled_command.h index 8bc5bf38838..e9a105366ac 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_link_collision_enabled_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_link_collision_enabled_command.h @@ -68,6 +68,5 @@ class ChangeLinkCollisionEnabledCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeLinkCollisionEnabledCommand, "ChangeLinkCollisionEnabledCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_LINK_COLLISION_ENABLED_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_link_origin_command.h b/tesseract_environment/include/tesseract_environment/commands/change_link_origin_command.h index 556ce0f42e3..9a36a041d32 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_link_origin_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_link_origin_command.h @@ -68,6 +68,5 @@ class ChangeLinkOriginCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeLinkOriginCommand, "ChangeLinkOriginCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_LINK_ORIGIN_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/change_link_visibility_command.h b/tesseract_environment/include/tesseract_environment/commands/change_link_visibility_command.h index 6b0bb77bf75..47fdc5d93bd 100644 --- a/tesseract_environment/include/tesseract_environment/commands/change_link_visibility_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/change_link_visibility_command.h @@ -68,6 +68,5 @@ class ChangeLinkVisibilityCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ChangeLinkVisibilityCommand, "ChangeLinkVisibilityCommand") #endif // TESSERACT_ENVIRONMENT_CHANGE_LINK_VISIBILITY_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/modify_allowed_collisions_command.h b/tesseract_environment/include/tesseract_environment/commands/modify_allowed_collisions_command.h index 72c34760559..ba4f4a19929 100644 --- a/tesseract_environment/include/tesseract_environment/commands/modify_allowed_collisions_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/modify_allowed_collisions_command.h @@ -71,7 +71,6 @@ class ModifyAllowedCollisionsCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ModifyAllowedCollisionsCommand, "ModifyAllowedCollisionsCommand") #endif // TESSERACT_ENVIRONMENT_MODIFY_ALLOWED_COLLISIONS_MATRIX_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/move_joint_command.h b/tesseract_environment/include/tesseract_environment/commands/move_joint_command.h index a4b1c8e1543..a13d2f7f372 100644 --- a/tesseract_environment/include/tesseract_environment/commands/move_joint_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/move_joint_command.h @@ -71,7 +71,6 @@ class MoveJointCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::MoveJointCommand, "MoveJointCommand") #endif // TESSERACT_ENVIRONMENT_MOVE_JOINT_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/move_link_command.h b/tesseract_environment/include/tesseract_environment/commands/move_link_command.h index 19b5e05eab8..ffea490b1f1 100644 --- a/tesseract_environment/include/tesseract_environment/commands/move_link_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/move_link_command.h @@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include namespace tesseract_environment { @@ -54,13 +54,13 @@ class MoveLinkCommand : public Command */ MoveLinkCommand(const tesseract_scene_graph::Joint& joint); - const tesseract_scene_graph::Joint::ConstPtr& getJoint() const; + const std::shared_ptr& getJoint() const; bool operator==(const MoveLinkCommand& rhs) const; bool operator!=(const MoveLinkCommand& rhs) const; private: - tesseract_scene_graph::Joint::ConstPtr joint_; + std::shared_ptr joint_; friend class boost::serialization::access; template @@ -69,6 +69,5 @@ class MoveLinkCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::MoveLinkCommand, "MoveLinkCommand") #endif // TESSERACT_ENVIRONMENT_MOVE_LINK_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/remove_allowed_collision_link_command.h b/tesseract_environment/include/tesseract_environment/commands/remove_allowed_collision_link_command.h index c0f0a0eff17..21cb7b466dc 100644 --- a/tesseract_environment/include/tesseract_environment/commands/remove_allowed_collision_link_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/remove_allowed_collision_link_command.h @@ -65,6 +65,5 @@ class RemoveAllowedCollisionLinkCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::RemoveAllowedCollisionLinkCommand, "RemoveAllowedCollisionLinkCommand") #endif // TESSERACT_ENVIRONMENT_REMOVE_ALLOWED_COLLISION_LINK_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/remove_joint_command.h b/tesseract_environment/include/tesseract_environment/commands/remove_joint_command.h index 2b1dd9374d1..504dbc265b2 100644 --- a/tesseract_environment/include/tesseract_environment/commands/remove_joint_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/remove_joint_command.h @@ -68,6 +68,5 @@ class RemoveJointCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::RemoveJointCommand, "RemoveJointCommand") #endif // TESSERACT_ENVIRONMENT_REMOVE_JOINT_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/remove_link_command.h b/tesseract_environment/include/tesseract_environment/commands/remove_link_command.h index fa492ce3c6e..b29b2afed95 100644 --- a/tesseract_environment/include/tesseract_environment/commands/remove_link_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/remove_link_command.h @@ -69,6 +69,5 @@ class RemoveLinkCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::RemoveLinkCommand, "RemoveLinkCommand") #endif // TESSERACT_ENVIRONMENT_REMOVE_LINK_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/replace_joint_command.h b/tesseract_environment/include/tesseract_environment/commands/replace_joint_command.h index 143ca2e4805..876343dc361 100644 --- a/tesseract_environment/include/tesseract_environment/commands/replace_joint_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/replace_joint_command.h @@ -32,7 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include namespace tesseract_environment { @@ -59,13 +59,13 @@ class ReplaceJointCommand : public Command */ ReplaceJointCommand(const tesseract_scene_graph::Joint& joint); - const tesseract_scene_graph::Joint::ConstPtr& getJoint() const; + const std::shared_ptr& getJoint() const; bool operator==(const ReplaceJointCommand& rhs) const; bool operator!=(const ReplaceJointCommand& rhs) const; private: - tesseract_scene_graph::Joint::ConstPtr joint_; + std::shared_ptr joint_; friend class boost::serialization::access; template @@ -75,6 +75,5 @@ class ReplaceJointCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ReplaceJointCommand, "ReplaceJointCommand") #endif // TESSERACT_ENVIRONMENT_REPLACE_JOINT_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/set_active_continuous_contact_manager_command.h b/tesseract_environment/include/tesseract_environment/commands/set_active_continuous_contact_manager_command.h index d4baafdf84d..ab5dfa17725 100644 --- a/tesseract_environment/include/tesseract_environment/commands/set_active_continuous_contact_manager_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/set_active_continuous_contact_manager_command.h @@ -66,7 +66,6 @@ class SetActiveContinuousContactManagerCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::SetActiveContinuousContactManagerCommand, "SetActiveContinuousContactManagerCommand") #endif // TESSERACT_ENVIRONMENT_SET_ACTIVE_CONTINUOUS_CONTACT_MANAGER_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/commands/set_active_discrete_contact_manager_command.h b/tesseract_environment/include/tesseract_environment/commands/set_active_discrete_contact_manager_command.h index 8d959e593c8..58401c15e2d 100644 --- a/tesseract_environment/include/tesseract_environment/commands/set_active_discrete_contact_manager_command.h +++ b/tesseract_environment/include/tesseract_environment/commands/set_active_discrete_contact_manager_command.h @@ -65,7 +65,6 @@ class SetActiveDiscreteContactManagerCommand : public Command } // namespace tesseract_environment #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_environment::SetActiveDiscreteContactManagerCommand, "SetActiveDiscreteContactManagerCommand") #endif // TESSERACT_ENVIRONMENT_SET_ACTIVE_DISCRETE_CONTACT_MANAGER_COMMAND_H diff --git a/tesseract_environment/include/tesseract_environment/environment.h b/tesseract_environment/include/tesseract_environment/environment.h index 40e1306eb3f..8cc86aece4d 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/tesseract_environment/include/tesseract_environment/environment.h @@ -25,32 +25,54 @@ */ #ifndef TESSERACT_ENVIRONMENT_ENVIRONMENT_H #define TESSERACT_ENVIRONMENT_ENVIRONMENT_H + #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include #include #include -#include #include -#include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + #include -#include -#include -#include -#include -#include -#include -#include -#include -#include + +// Should be able to remove these by storing them as shared pointer types +#include #include +#include +#include + +// #include + +// #include +// #include +// #include + +// #include + +// #include +// #include +// #include +// #include +// #include +// #include +// #include namespace tesseract_environment { @@ -61,6 +83,8 @@ namespace tesseract_environment */ using FindTCPOffsetCallbackFn = std::function; +using EventCallbackFn = std::function; + class Environment { public: @@ -89,7 +113,7 @@ class Environment * @param scene_graph The scene graph to initialize the environment. * @return True if successful, otherwise false */ - bool init(const Commands& commands); + bool init(const std::vector>& commands); /** * @brief Initialize the Environment @@ -100,19 +124,20 @@ class Environment * @return True if successful, otherwise false */ bool init(const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_srdf::SRDFModel::ConstPtr& srdf_model = nullptr); + const std::shared_ptr& srdf_model = nullptr); - bool init(const std::string& urdf_string, const tesseract_common::ResourceLocator::ConstPtr& locator); + bool init(const std::string& urdf_string, const std::shared_ptr& locator); bool init(const std::string& urdf_string, const std::string& srdf_string, - const tesseract_common::ResourceLocator::ConstPtr& locator); + const std::shared_ptr& locator); - bool init(const tesseract_common::fs::path& urdf_path, const tesseract_common::ResourceLocator::ConstPtr& locator); + bool init(const tesseract_common::fs::path& urdf_path, + const std::shared_ptr& locator); bool init(const tesseract_common::fs::path& urdf_path, const tesseract_common::fs::path& srdf_path, - const tesseract_common::ResourceLocator::ConstPtr& locator); + const std::shared_ptr& locator); /** * @brief Clone the environment @@ -143,7 +168,7 @@ class Environment * @brief Get Environment command history post initialization * @return List of commands */ - Commands getCommandHistory() const; + std::vector> getCommandHistory() const; /** * @brief Applies the commands to the environment @@ -151,7 +176,7 @@ class Environment * @return true if successful. If returned false, then only a partial set of commands have been applied. Call * getCommandHistory to check. Some commands are not checked for success */ - bool applyCommands(const Commands& commands); + bool applyCommands(const std::vector>& commands); /** * @brief Apply command to the environment @@ -159,13 +184,13 @@ class Environment * @return true if successful. If returned false, then the command have not been applied. * Some type of Command are not checked for success */ - bool applyCommand(Command::ConstPtr command); + bool applyCommand(std::shared_ptr command); /** * @brief Get the Scene Graph * @return SceneGraphConstPtr */ - tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const; + std::shared_ptr getSceneGraph() const; /** * @brief Get a groups joint names @@ -179,7 +204,7 @@ class Environment * @param group_name The group name * @return A joint group */ - tesseract_kinematics::JointGroup::UPtr getJointGroup(const std::string& group_name) const; + std::unique_ptr getJointGroup(const std::string& group_name) const; /** * @brief Get a joint group given a vector of joint names @@ -187,8 +212,8 @@ class Environment * @param joint_names The joint names that make up the group * @return A joint group */ - tesseract_kinematics::JointGroup::UPtr getJointGroup(const std::string& name, - const std::vector& joint_names) const; + std::unique_ptr getJointGroup(const std::string& name, + const std::vector& joint_names) const; /** * @brief Get a kinematic group given group name and solver name @@ -197,8 +222,8 @@ class Environment * @param ik_solver_name The IK solver name * @return A kinematics group */ - tesseract_kinematics::KinematicGroup::UPtr getKinematicGroup(const std::string& group_name, - std::string ik_solver_name = "") const; + std::unique_ptr getKinematicGroup(const std::string& group_name, + std::string ik_solver_name = "") const; /** * @brief Find tool center point provided in the manipulator info @@ -257,14 +282,14 @@ class Environment * @brief Set resource locator for environment * @param locator The resource locator */ - void setResourceLocator(tesseract_common::ResourceLocator::ConstPtr locator); + void setResourceLocator(std::shared_ptr locator); /** * @brief Get the resource locator assigned * @details This can be a nullptr * @return The resource locator assigned to the environment */ - tesseract_common::ResourceLocator::ConstPtr getResourceLocator() const; + std::shared_ptr getResourceLocator() const; /** @brief Give the environment a name */ void setName(const std::string& name); @@ -311,21 +336,21 @@ class Environment * @param name The name of the link * @return Return nullptr if link name does not exists, otherwise a pointer to the link */ - tesseract_scene_graph::Link::ConstPtr getLink(const std::string& name) const; + std::shared_ptr getLink(const std::string& name) const; /** * @brief Get joint by name * @param name The name of the joint * @return Joint Const Pointer */ - tesseract_scene_graph::Joint::ConstPtr getJoint(const std::string& name) const; + std::shared_ptr getJoint(const std::string& name) const; /** * @brief Gets the limits associated with a joint * @param joint_name Name of the joint to be updated * @return The joint limits set for the given joint */ - tesseract_scene_graph::JointLimits::ConstPtr getJointLimits(const std::string& joint_name) const; + std::shared_ptr getJointLimits(const std::string& joint_name) const; /** * @brief Get whether a link should be considered during collision checking @@ -343,7 +368,7 @@ class Environment * @brief Get the allowed collision matrix * @return AllowedCollisionMatrixConstPtr */ - tesseract_common::AllowedCollisionMatrix::ConstPtr getAllowedCollisionMatrix() const; + std::shared_ptr getAllowedCollisionMatrix() const; /** * @brief Get a vector of joint names in the environment @@ -444,7 +469,7 @@ class Environment * * @return A clone of the environments state solver */ - tesseract_scene_graph::StateSolver::UPtr getStateSolver() const; + std::unique_ptr getStateSolver() const; /** * @brief Get the kinematics information @@ -456,7 +481,7 @@ class Environment * @brief Get the available group names * @return The group names */ - tesseract_srdf::GroupNames getGroupNames() const; + std::set getGroupNames() const; /** * @brief Get the contact managers plugin information @@ -472,7 +497,7 @@ class Environment bool setActiveDiscreteContactManager(const std::string& name); /** @brief Get a copy of the environments active discrete contact manager */ - tesseract_collision::DiscreteContactManager::UPtr getDiscreteContactManager() const; + std::unique_ptr getDiscreteContactManager() const; /** * @brief Set the cached internal copy of the environments active discrete contact manager not nullptr @@ -481,7 +506,7 @@ class Environment void clearCachedDiscreteContactManager() const; /** @brief Get a copy of the environments available discrete contact manager by name */ - tesseract_collision::DiscreteContactManager::UPtr getDiscreteContactManager(const std::string& name) const; + std::unique_ptr getDiscreteContactManager(const std::string& name) const; /** * @brief Set the active continuous contact manager @@ -491,7 +516,7 @@ class Environment bool setActiveContinuousContactManager(const std::string& name); /** @brief Get a copy of the environments active continuous contact manager */ - tesseract_collision::ContinuousContactManager::UPtr getContinuousContactManager() const; + std::unique_ptr getContinuousContactManager() const; /** * @brief Set the cached internal copy of the environments active continuous contact manager not nullptr @@ -500,7 +525,8 @@ class Environment void clearCachedContinuousContactManager() const; /** @brief Get a copy of the environments available continuous contact manager by name */ - tesseract_collision::ContinuousContactManager::UPtr getContinuousContactManager(const std::string& name) const; + std::unique_ptr + getContinuousContactManager(const std::string& name) const; /** @brief Get the environment collision margin data */ tesseract_common::CollisionMarginData getCollisionMarginData() const; @@ -537,13 +563,13 @@ class Environment int init_revision_{ 0 }; /** @brief The history of commands applied to the environment after initialization */ - Commands commands_{}; + std::vector> commands_{}; /** * @brief Tesseract Scene Graph * @note This is intentionally not serialized it will auto updated */ - tesseract_scene_graph::SceneGraph::Ptr scene_graph_{ nullptr }; + std::shared_ptr scene_graph_{ nullptr }; /** * @brief The kinematics information @@ -570,13 +596,13 @@ class Environment * @brief Tesseract State Solver * @note This is intentionally not serialized it will auto updated */ - tesseract_scene_graph::MutableStateSolver::UPtr state_solver_{ nullptr }; + std::unique_ptr state_solver_{ nullptr }; /** * @brief The function used to determine if two objects are allowed in collision * @todo This needs to be switched to class so it may be serialized */ - tesseract_collision::IsContactAllowedFn is_contact_allowed_fn_; + std::function is_contact_allowed_fn_; /** * @brief A vector of user defined callbacks for locating tool center point @@ -591,7 +617,7 @@ class Environment std::map event_cb_{}; /** @brief Used when initialized by URDF_STRING, URDF_STRING_SRDF_STRING, URDF_PATH, and URDF_PATH_SRDF_PATH */ - tesseract_common::ResourceLocator::ConstPtr resource_locator_{ nullptr }; + std::shared_ptr resource_locator_{ nullptr }; /** * @brief The contact manager information @@ -609,20 +635,20 @@ class Environment * @brief The collision margin data * @note This is intentionally not serialized it will auto updated */ - tesseract_collision::CollisionMarginData collision_margin_data_; + tesseract_common::CollisionMarginData collision_margin_data_; /** * @brief The discrete contact manager object * @note This is intentionally not serialized it will auto updated */ - mutable tesseract_collision::DiscreteContactManager::UPtr discrete_manager_{ nullptr }; + mutable std::unique_ptr discrete_manager_{ nullptr }; mutable std::shared_mutex discrete_manager_mutex_; /** * @brief The continuous contact manager object * @note This is intentionally not serialized it will auto updated */ - mutable tesseract_collision::ContinuousContactManager::UPtr continuous_manager_{ nullptr }; + mutable std::unique_ptr continuous_manager_{ nullptr }; mutable std::shared_mutex continuous_manager_mutex_; /** @@ -638,7 +664,7 @@ class Environment * @details This will cleared when environment changes * @note This is intentionally not serialized it will auto updated */ - mutable std::unordered_map joint_group_cache_{}; + mutable std::unordered_map> joint_group_cache_{}; mutable std::shared_mutex joint_group_cache_mutex_; /** @@ -646,7 +672,7 @@ class Environment * @details This will cleared when environment changes * @note This is intentionally not serialized it will auto updated */ - mutable std::map, tesseract_kinematics::KinematicGroup::UPtr> + mutable std::map, std::unique_ptr> kinematic_group_cache_{}; mutable std::shared_mutex kinematic_group_cache_mutex_; @@ -674,50 +700,56 @@ class Environment private: bool removeLinkHelper(const std::string& name); - static void getCollisionObject(tesseract_collision::CollisionShapesConst& shapes, + static void getCollisionObject(std::vector>& shapes, tesseract_common::VectorIsometry3d& shape_poses, const tesseract_scene_graph::Link& link); bool setActiveDiscreteContactManagerHelper(const std::string& name); bool setActiveContinuousContactManagerHelper(const std::string& name); - tesseract_collision::DiscreteContactManager::UPtr getDiscreteContactManagerHelper(const std::string& name) const; + std::unique_ptr + getDiscreteContactManagerHelper(const std::string& name) const; - tesseract_collision::ContinuousContactManager::UPtr getContinuousContactManagerHelper(const std::string& name) const; + std::unique_ptr + getContinuousContactManagerHelper(const std::string& name) const; - bool initHelper(const Commands& commands); - static Commands getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_srdf::SRDFModel::ConstPtr& srdf_model = nullptr); + bool initHelper(const std::vector>& commands); + static std::vector> + getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::shared_ptr& srdf_model = nullptr); /** @brief Apply Command Helper which does not lock */ - bool applyCommandsHelper(const Commands& commands); + bool applyCommandsHelper(const std::vector>& commands); // Command Helper function - bool applyAddCommand(const AddLinkCommand::ConstPtr& cmd); - bool applyMoveLinkCommand(const MoveLinkCommand::ConstPtr& cmd); - bool applyMoveJointCommand(const MoveJointCommand::ConstPtr& cmd); - bool applyRemoveLinkCommand(const RemoveLinkCommand::ConstPtr& cmd); - bool applyRemoveJointCommand(const RemoveJointCommand::ConstPtr& cmd); - bool applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& cmd); - bool applyChangeLinkOriginCommand(const ChangeLinkOriginCommand::ConstPtr& cmd); - bool applyChangeJointOriginCommand(const ChangeJointOriginCommand::ConstPtr& cmd); - bool applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisionEnabledCommand::ConstPtr& cmd); - bool applyChangeLinkVisibilityCommand(const ChangeLinkVisibilityCommand::ConstPtr& cmd); - bool applyModifyAllowedCollisionsCommand(const ModifyAllowedCollisionsCommand::ConstPtr& cmd); - bool applyRemoveAllowedCollisionLinkCommand(const RemoveAllowedCollisionLinkCommand::ConstPtr& cmd); - bool applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd); - bool applyChangeJointPositionLimitsCommand(const ChangeJointPositionLimitsCommand::ConstPtr& cmd); - bool applyChangeJointVelocityLimitsCommand(const ChangeJointVelocityLimitsCommand::ConstPtr& cmd); - bool applyChangeJointAccelerationLimitsCommand(const ChangeJointAccelerationLimitsCommand::ConstPtr& cmd); - bool applyAddKinematicsInformationCommand(const AddKinematicsInformationCommand::ConstPtr& cmd); - bool applyChangeCollisionMarginsCommand(const ChangeCollisionMarginsCommand::ConstPtr& cmd); - bool applyAddContactManagersPluginInfoCommand(const AddContactManagersPluginInfoCommand::ConstPtr& cmd); - bool applySetActiveContinuousContactManagerCommand(const SetActiveContinuousContactManagerCommand::ConstPtr& cmd); - bool applySetActiveDiscreteContactManagerCommand(const SetActiveDiscreteContactManagerCommand::ConstPtr& cmd); - bool applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand::ConstPtr& cmd); - - bool applyAddLinkCommandHelper(const tesseract_scene_graph::Link::ConstPtr& link, - const tesseract_scene_graph::Joint::ConstPtr& joint, + bool applyAddCommand(const std::shared_ptr& cmd); + bool applyMoveLinkCommand(const std::shared_ptr& cmd); + bool applyMoveJointCommand(const std::shared_ptr& cmd); + bool applyRemoveLinkCommand(const std::shared_ptr& cmd); + bool applyRemoveJointCommand(const std::shared_ptr& cmd); + bool applyReplaceJointCommand(const std::shared_ptr& cmd); + bool applyChangeLinkOriginCommand(const std::shared_ptr& cmd); + bool applyChangeJointOriginCommand(const std::shared_ptr& cmd); + bool applyChangeLinkCollisionEnabledCommand(const std::shared_ptr& cmd); + bool applyChangeLinkVisibilityCommand(const std::shared_ptr& cmd); + bool applyModifyAllowedCollisionsCommand(const std::shared_ptr& cmd); + bool applyRemoveAllowedCollisionLinkCommand(const std::shared_ptr& cmd); + bool applyAddSceneGraphCommand(std::shared_ptr cmd); + bool applyChangeJointPositionLimitsCommand(const std::shared_ptr& cmd); + bool applyChangeJointVelocityLimitsCommand(const std::shared_ptr& cmd); + bool + applyChangeJointAccelerationLimitsCommand(const std::shared_ptr& cmd); + bool applyAddKinematicsInformationCommand(const std::shared_ptr& cmd); + bool applyChangeCollisionMarginsCommand(const std::shared_ptr& cmd); + bool applyAddContactManagersPluginInfoCommand(const std::shared_ptr& cmd); + bool applySetActiveContinuousContactManagerCommand( + const std::shared_ptr& cmd); + bool + applySetActiveDiscreteContactManagerCommand(const std::shared_ptr& cmd); + bool applyAddTrajectoryLinkCommand(const std::shared_ptr& cmd); + + bool applyAddLinkCommandHelper(const std::shared_ptr& link, + const std::shared_ptr& joint, bool replace_allowed); friend class boost::serialization::access; diff --git a/tesseract_environment/include/tesseract_environment/environment_cache.h b/tesseract_environment/include/tesseract_environment/environment_cache.h index b88e9faf256..d87584c9518 100644 --- a/tesseract_environment/include/tesseract_environment/environment_cache.h +++ b/tesseract_environment/include/tesseract_environment/environment_cache.h @@ -29,12 +29,13 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include namespace tesseract_environment { +class Environment; class EnvironmentCache { public: @@ -69,7 +70,7 @@ class EnvironmentCache * @brief This will pop an Environment object from the queue * @details This will first call refreshCache to ensure it has an updated tesseract then proceed */ - virtual Environment::UPtr getCachedEnvironment() const = 0; + virtual std::unique_ptr getCachedEnvironment() const = 0; }; class DefaultEnvironmentCache : public EnvironmentCache @@ -78,7 +79,7 @@ class DefaultEnvironmentCache : public EnvironmentCache using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - DefaultEnvironmentCache(Environment::ConstPtr env, std::size_t cache_size = 5); + DefaultEnvironmentCache(std::shared_ptr env, std::size_t cache_size = 5); /** * @brief Set the cache size used to hold tesseract objects for motion planning @@ -99,11 +100,11 @@ class DefaultEnvironmentCache : public EnvironmentCache * @brief This will pop an Environment object from the queue * @details This will first call refreshCache to ensure it has an updated tesseract then proceed */ - Environment::UPtr getCachedEnvironment() const override final; + std::unique_ptr getCachedEnvironment() const override final; protected: /** @brief The tesseract_object used to create the cache */ - Environment::ConstPtr env_; + std::shared_ptr env_; /** @brief The assigned cache size */ std::size_t cache_size_{ 5 }; @@ -112,7 +113,7 @@ class DefaultEnvironmentCache : public EnvironmentCache mutable int cache_env_revision_{ 0 }; /** @brief A vector of cached Tesseract objects */ - mutable std::deque cache_; + mutable std::deque> cache_; /** @brief The mutex used when reading and writing to cache_ */ mutable std::shared_mutex cache_mutex_; diff --git a/tesseract_environment/include/tesseract_environment/environment_monitor.h b/tesseract_environment/include/tesseract_environment/environment_monitor.h index 0ab778f106b..ba836cd11e8 100644 --- a/tesseract_environment/include/tesseract_environment/environment_monitor.h +++ b/tesseract_environment/include/tesseract_environment/environment_monitor.h @@ -26,11 +26,16 @@ #ifndef TESSERACT_ENVIRONMENT_ENVIRONMENT_MONITOR_H #define TESSERACT_ENVIRONMENT_ENVIRONMENT_MONITOR_H -#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_environment { +class Environment; + enum class MonitoredEnvironmentMode : int { /** @@ -70,22 +75,19 @@ class EnvironmentMonitor * @brief Constructor * @param monitor_namespace A name identifying this monitor, must be unique */ - EnvironmentMonitor(std::string monitor_namespace) : monitor_namespace_(monitor_namespace) {} + EnvironmentMonitor(std::string monitor_namespace); /** * @brief Constructor * @param env The environment to use internal to the monitor * @param monitor_namespace A name identifying this monitor, must be unique */ - EnvironmentMonitor(tesseract_environment::Environment::Ptr env, std::string monitor_namespace) - : env_(std::move(env)), monitor_namespace_(monitor_namespace) - { - } + EnvironmentMonitor(std::shared_ptr env, std::string monitor_namespace); virtual ~EnvironmentMonitor() = default; /** \brief Get the namespace of this monitor */ - virtual const std::string& getNamespace() const { return monitor_namespace_; } + virtual const std::string& getNamespace() const; /** * @brief Returns an @b threadsafe reference to the current environment. @@ -94,12 +96,12 @@ class EnvironmentMonitor * commands to the monitored environment until the todo below is implemented. * @return The current environment. */ - virtual tesseract_environment::Environment& environment() { return *env_; } + virtual Environment& environment(); /** * @brief Returns an @b threadsafe const reference to the current environment. * @return The current environment.*/ - virtual const tesseract_environment::Environment& environment() const { return *env_; } + virtual const Environment& environment() const; /** * @brief Returns an @b threadsafe shared pointer to the current environment. @@ -108,13 +110,13 @@ class EnvironmentMonitor * commands to the monitored environment until the todo below is implemented. * @return The current environment. */ - virtual tesseract_environment::Environment::Ptr getEnvironment() { return env_; } + virtual std::shared_ptr getEnvironment(); /** * @brief Returns an @b threadsafe const shared point to the current environment. * @return The current environment. */ - virtual tesseract_environment::Environment::ConstPtr getEnvironment() const { return env_; } + virtual std::shared_ptr getEnvironment() const; /** * @brief Wait for connection to upstream environment @@ -185,7 +187,7 @@ class EnvironmentMonitor virtual void shutdown() = 0; protected: - tesseract_environment::Environment::Ptr env_; + std::shared_ptr env_; std::string monitor_namespace_; MonitoredEnvironmentMode mode_{ MonitoredEnvironmentMode::DEFAULT }; }; diff --git a/tesseract_environment/include/tesseract_environment/environment_monitor_interface.h b/tesseract_environment/include/tesseract_environment/environment_monitor_interface.h index 8219872c11d..a77763d3770 100644 --- a/tesseract_environment/include/tesseract_environment/environment_monitor_interface.h +++ b/tesseract_environment/include/tesseract_environment/environment_monitor_interface.h @@ -29,13 +29,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include namespace tesseract_environment { +class Command; +class Environment; + class EnvironmentMonitorInterface { public: @@ -44,7 +48,7 @@ class EnvironmentMonitorInterface using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - EnvironmentMonitorInterface(std::string env_name) : env_name_(std::move(env_name)) {} + explicit EnvironmentMonitorInterface(std::string env_name) : env_name_(std::move(env_name)) {} virtual ~EnvironmentMonitorInterface() = default; @@ -81,9 +85,9 @@ class EnvironmentMonitorInterface * @param command The command to apply * @return A vector of failed namespace, if empty all namespace were updated successfully. */ - virtual std::vector applyCommand(const tesseract_environment::Command& command) const = 0; - virtual std::vector applyCommands(const tesseract_environment::Commands& commands) const = 0; - virtual std::vector applyCommands(const std::vector& commands) const = 0; + virtual std::vector applyCommand(const std::vector>& command) const = 0; + virtual std::vector applyCommands(const std::vector>& commands) const = 0; + virtual std::vector applyCommands(const std::vector& commands) const = 0; /** * @brief Apply provided command to only the provided namespace. The namespace does not have to be one that is @@ -91,12 +95,10 @@ class EnvironmentMonitorInterface * @param command The command to apply * @return True if successful, otherwise false */ - virtual bool applyCommand(const std::string& monitor_namespace, - const tesseract_environment::Command& command) const = 0; - virtual bool applyCommands(const std::string& monitor_namespace, - const tesseract_environment::Commands& commands) const = 0; + virtual bool applyCommand(const std::string& monitor_namespace, const Command& command) const = 0; virtual bool applyCommands(const std::string& monitor_namespace, - const std::vector& commands) const = 0; + const std::vector>& commands) const = 0; + virtual bool applyCommands(const std::string& monitor_namespace, const std::vector& commands) const = 0; /** * @brief Pull current environment state from the environment in the provided namespace @@ -134,7 +136,7 @@ class EnvironmentMonitorInterface * @param monitor_namespace The namespace to extract the environment from. * @return Environment Shared Pointer, if nullptr it failed */ - virtual tesseract_environment::Environment::UPtr getEnvironment(const std::string& monitor_namespace) const = 0; + virtual std::unique_ptr getEnvironment(const std::string& monitor_namespace) const = 0; protected: std::string env_name_; diff --git a/tesseract_environment/include/tesseract_environment/events.h b/tesseract_environment/include/tesseract_environment/events.h index 59e5a0d10d0..9e6a74dc9ac 100644 --- a/tesseract_environment/include/tesseract_environment/events.h +++ b/tesseract_environment/include/tesseract_environment/events.h @@ -25,11 +25,19 @@ */ #ifndef TESSERACT_ENVIRONMENT_EVENTS_H #define TESSERACT_ENVIRONMENT_EVENTS_H -#include -#include + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include namespace tesseract_environment { +class Command; + enum class Events { COMMAND_APPLIED = 0, @@ -39,7 +47,7 @@ enum class Events /** @brief The event base class */ struct Event { - Event(Events type) : type(type) {} + Event(Events type); virtual ~Event() = default; const Events type; @@ -51,12 +59,9 @@ struct Event */ struct CommandAppliedEvent : public Event { - CommandAppliedEvent(const Commands& commands, int revision) - : Event(Events::COMMAND_APPLIED), commands(commands), revision(revision) - { - } + CommandAppliedEvent(const std::vector>& commands, int revision); - const Commands& commands; + const std::vector>& commands; int revision; }; @@ -66,15 +71,11 @@ struct CommandAppliedEvent : public Event */ struct SceneStateChangedEvent : public Event { - SceneStateChangedEvent(const tesseract_scene_graph::SceneState& state) - : Event(Events::SCENE_STATE_CHANGED), state(state) - { - } + SceneStateChangedEvent(const tesseract_scene_graph::SceneState& state); const tesseract_scene_graph::SceneState& state; }; -using EventCallbackFn = std::function; } // namespace tesseract_environment #endif // TESSERACT_ENVIRONMENT_EVENTS_H diff --git a/tesseract_environment/include/tesseract_environment/fwd.h b/tesseract_environment/include/tesseract_environment/fwd.h new file mode 100644 index 00000000000..f0e30d2672c --- /dev/null +++ b/tesseract_environment/include/tesseract_environment/fwd.h @@ -0,0 +1,63 @@ +/** + * @file fwd.h + * @brief Tesseract Environment Forward Declarations. + * + * @author Levi Armstrong + * @date February 21, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_ENVIRONMENT_FWD_H +#define TESSERACT_ENVIRONMENT_FWD_H + +namespace tesseract_environment +{ +class Environment; +class EnvironmentCache; +class EnvironmentMonitorInterface; +class EnvironmentMonitor; +enum class MonitoredEnvironmentMode; +class Event; +class Command; +class AddLinkCommand; +class MoveLinkCommand; +class MoveJointCommand; +class RemoveLinkCommand; +class RemoveJointCommand; +class ReplaceJointCommand; +class ChangeLinkOriginCommand; +class ChangeJointOriginCommand; +class ChangeLinkCollisionEnabledCommand; +class ChangeLinkVisibilityCommand; +enum class ModifyAllowedCollisionsType; +class ModifyAllowedCollisionsCommand; +class RemoveAllowedCollisionLinkCommand; +class AddSceneGraphCommand; +class ChangeJointPositionLimitsCommand; +class ChangeJointVelocityLimitsCommand; +class ChangeJointAccelerationLimitsCommand; +class AddKinematicsInformationCommand; +class ChangeCollisionMarginsCommand; +class AddContactManagersPluginInfoCommand; +class SetActiveContinuousContactManagerCommand; +class SetActiveDiscreteContactManagerCommand; +class AddTrajectoryLinkCommand; +} // namespace tesseract_environment + +#endif // TESSERACT_ENVIRONMENT_FWD_H diff --git a/tesseract_environment/include/tesseract_environment/utils.h b/tesseract_environment/include/tesseract_environment/utils.h index d4dbecbaf55..aee5c7a27f5 100644 --- a/tesseract_environment/include/tesseract_environment/utils.h +++ b/tesseract_environment/include/tesseract_environment/utils.h @@ -26,12 +26,18 @@ #ifndef TESSERACT_ENVIRONMENT_CORE_UTILS_H #define TESSERACT_ENVIRONMENT_CORE_UTILS_H -#include -#include -#include -#include -#include -#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +#include namespace tesseract_environment { diff --git a/tesseract_environment/src/commands/add_link_command.cpp b/tesseract_environment/src/commands/add_link_command.cpp index 2475760ec57..8b1db103afa 100644 --- a/tesseract_environment/src/commands/add_link_command.cpp +++ b/tesseract_environment/src/commands/add_link_command.cpp @@ -31,8 +31,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include +#include namespace tesseract_environment { @@ -61,8 +63,8 @@ AddLinkCommand::AddLinkCommand(const tesseract_scene_graph::Link& link, /** @todo if joint is not fixed we should verify that limits are provided */ } -const tesseract_scene_graph::Link::ConstPtr& AddLinkCommand::getLink() const { return link_; } -const tesseract_scene_graph::Joint::ConstPtr& AddLinkCommand::getJoint() const { return joint_; } +const std::shared_ptr& AddLinkCommand::getLink() const { return link_; } +const std::shared_ptr& AddLinkCommand::getJoint() const { return joint_; } bool AddLinkCommand::replaceAllowed() const { return replace_allowed_; } bool AddLinkCommand::operator==(const AddLinkCommand& rhs) const diff --git a/tesseract_environment/src/commands/add_scene_graph_command.cpp b/tesseract_environment/src/commands/add_scene_graph_command.cpp index d118d369703..e10edb6d295 100644 --- a/tesseract_environment/src/commands/add_scene_graph_command.cpp +++ b/tesseract_environment/src/commands/add_scene_graph_command.cpp @@ -32,8 +32,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include +#include namespace tesseract_environment { @@ -57,8 +59,11 @@ AddSceneGraphCommand::AddSceneGraphCommand(const tesseract_scene_graph::SceneGra { } -const tesseract_scene_graph::SceneGraph::ConstPtr& AddSceneGraphCommand::getSceneGraph() const { return scene_graph_; } -const tesseract_scene_graph::Joint::ConstPtr& AddSceneGraphCommand::getJoint() const { return joint_; } +const std::shared_ptr& AddSceneGraphCommand::getSceneGraph() const +{ + return scene_graph_; +} +const std::shared_ptr& AddSceneGraphCommand::getJoint() const { return joint_; } const std::string& AddSceneGraphCommand::getPrefix() const { return prefix_; } bool AddSceneGraphCommand::operator==(const AddSceneGraphCommand& rhs) const diff --git a/tesseract_environment/src/commands/move_link_command.cpp b/tesseract_environment/src/commands/move_link_command.cpp index 4145b5cb579..0c0f7f3ab71 100644 --- a/tesseract_environment/src/commands/move_link_command.cpp +++ b/tesseract_environment/src/commands/move_link_command.cpp @@ -31,8 +31,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tesseract_environment { diff --git a/tesseract_environment/src/commands/replace_joint_command.cpp b/tesseract_environment/src/commands/replace_joint_command.cpp index 4d8c6e6185e..0c4acd2ea55 100644 --- a/tesseract_environment/src/commands/replace_joint_command.cpp +++ b/tesseract_environment/src/commands/replace_joint_command.cpp @@ -31,8 +31,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tesseract_environment { diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp index 714c017fea4..b48c43286a0 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -25,12 +25,6 @@ */ #include -#include -#include -#include -#include -#include - TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include @@ -38,9 +32,41 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + namespace tesseract_environment { -bool Environment::initHelper(const Commands& commands) +bool Environment::initHelper(const std::vector>& commands) { if (commands.empty()) return false; @@ -75,7 +101,7 @@ bool Environment::initHelper(const Commands& commands) return initialized_; } -bool Environment::init(const Commands& commands) +bool Environment::init(const std::vector>& commands) { bool success{ false }; { @@ -92,13 +118,14 @@ bool Environment::init(const Commands& commands) } bool Environment::init(const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_srdf::SRDFModel::ConstPtr& srdf_model) + const std::shared_ptr& srdf_model) { Commands commands = getInitCommands(scene_graph, srdf_model); return init(commands); } -bool Environment::init(const std::string& urdf_string, const tesseract_common::ResourceLocator::ConstPtr& locator) +bool Environment::init(const std::string& urdf_string, + const std::shared_ptr& locator) { { std::unique_lock lock(mutex_); @@ -124,7 +151,7 @@ bool Environment::init(const std::string& urdf_string, const tesseract_common::R bool Environment::init(const std::string& urdf_string, const std::string& srdf_string, - const tesseract_common::ResourceLocator::ConstPtr& locator) + const std::shared_ptr& locator) { { std::unique_lock lock(mutex_); @@ -162,7 +189,7 @@ bool Environment::init(const std::string& urdf_string, } bool Environment::init(const tesseract_common::fs::path& urdf_path, - const tesseract_common::ResourceLocator::ConstPtr& locator) + const std::shared_ptr& locator) { { std::unique_lock lock(mutex_); @@ -188,7 +215,7 @@ bool Environment::init(const tesseract_common::fs::path& urdf_path, bool Environment::init(const tesseract_common::fs::path& urdf_path, const tesseract_common::fs::path& srdf_path, - const tesseract_common::ResourceLocator::ConstPtr& locator) + const std::shared_ptr& locator) { { std::unique_lock lock(mutex_); @@ -292,8 +319,9 @@ void Environment::clear() } } -Commands Environment::getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_srdf::SRDFModel::ConstPtr& srdf_model) +std::vector> +Environment::getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::shared_ptr& srdf_model) { Commands commands; @@ -345,13 +373,13 @@ int Environment::getRevision() const return revision_; } -Commands Environment::getCommandHistory() const +std::vector> Environment::getCommandHistory() const { std::shared_lock lock(mutex_); return commands_; } -bool Environment::applyCommands(const Commands& commands) +bool Environment::applyCommands(const std::vector>& commands) { bool success{ false }; { @@ -365,9 +393,9 @@ bool Environment::applyCommands(const Commands& commands) return success; } -bool Environment::applyCommand(Command::ConstPtr command) { return applyCommands({ std::move(command) }); } +bool Environment::applyCommand(std::shared_ptr command) { return applyCommands({ std::move(command) }); } -tesseract_scene_graph::SceneGraph::ConstPtr Environment::getSceneGraph() const { return scene_graph_; } +std::shared_ptr Environment::getSceneGraph() const { return scene_graph_; } std::vector Environment::getGroupJointNames(const std::string& group_name) const { @@ -414,7 +442,7 @@ std::vector Environment::getGroupJointNames(const std::string& grou throw std::runtime_error("Environment, failed to get group '" + group_name + "' joint names!"); } -tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& group_name) const +std::unique_ptr Environment::getJointGroup(const std::string& group_name) const { std::shared_lock lock(mutex_); @@ -435,15 +463,15 @@ tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::str return jg; } -tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& name, - const std::vector& joint_names) const +std::unique_ptr +Environment::getJointGroup(const std::string& name, const std::vector& joint_names) const { std::shared_lock lock(mutex_); return std::make_unique(name, joint_names, *scene_graph_, current_state_); } -tesseract_kinematics::KinematicGroup::UPtr Environment::getKinematicGroup(const std::string& group_name, - std::string ik_solver_name) const +std::unique_ptr Environment::getKinematicGroup(const std::string& group_name, + std::string ik_solver_name) const { std::shared_lock lock(mutex_); @@ -560,13 +588,13 @@ std::map Environment::getEventCallbacks() const return event_cb_; } -void Environment::setResourceLocator(tesseract_common::ResourceLocator::ConstPtr locator) +void Environment::setResourceLocator(std::shared_ptr locator) { std::unique_lock lock(mutex_); resource_locator_ = std::move(locator); } -tesseract_common::ResourceLocator::ConstPtr Environment::getResourceLocator() const +std::shared_ptr Environment::getResourceLocator() const { std::shared_lock lock(mutex_); return resource_locator_; @@ -640,14 +668,15 @@ std::chrono::system_clock::time_point Environment::getCurrentStateTimestamp() co return current_state_timestamp_; } -tesseract_scene_graph::Link::ConstPtr Environment::getLink(const std::string& name) const +std::shared_ptr Environment::getLink(const std::string& name) const { std::shared_lock lock(mutex_); tesseract_scene_graph::Link::ConstPtr link = scene_graph_->getLink(name); return link; } -tesseract_scene_graph::JointLimits::ConstPtr Environment::getJointLimits(const std::string& joint_name) const +std::shared_ptr +Environment::getJointLimits(const std::string& joint_name) const { std::shared_lock lock(mutex_); return scene_graph_->getJointLimits(joint_name); @@ -665,7 +694,7 @@ bool Environment::getLinkVisibility(const std::string& name) const return scene_graph_->getLinkVisibility(name); } -tesseract_common::AllowedCollisionMatrix::ConstPtr Environment::getAllowedCollisionMatrix() const +std::shared_ptr Environment::getAllowedCollisionMatrix() const { std::shared_lock lock(mutex_); return scene_graph_->getAllowedCollisionMatrix(); @@ -683,7 +712,7 @@ std::vector Environment::getActiveJointNames() const return state_solver_->getActiveJointNames(); } -tesseract_scene_graph::Joint::ConstPtr Environment::getJoint(const std::string& name) const +std::shared_ptr Environment::getJoint(const std::string& name) const { std::shared_lock lock(mutex_); return scene_graph_->getJoint(name); @@ -781,7 +810,7 @@ Eigen::Isometry3d Environment::getRelativeLinkTransform(const std::string& from_ return state_solver_->getRelativeLinkTransform(from_link_name, to_link_name); } -tesseract_scene_graph::StateSolver::UPtr Environment::getStateSolver() const +std::unique_ptr Environment::getStateSolver() const { std::shared_lock lock(mutex_); return state_solver_->clone(); @@ -793,7 +822,7 @@ tesseract_srdf::KinematicsInformation Environment::getKinematicsInformation() co return kinematics_information_; } -tesseract_srdf::GroupNames Environment::getGroupNames() const +std::set Environment::getGroupNames() const { std::shared_lock lock(mutex_); return kinematics_information_.group_names; @@ -812,7 +841,8 @@ bool Environment::setActiveDiscreteContactManager(const std::string& name) return setActiveDiscreteContactManagerHelper(name); } -tesseract_collision::DiscreteContactManager::UPtr Environment::getDiscreteContactManager(const std::string& name) const +std::unique_ptr +Environment::getDiscreteContactManager(const std::string& name) const { std::shared_lock lock(mutex_); tesseract_collision::DiscreteContactManager::UPtr manager = getDiscreteContactManagerHelper(name); @@ -832,7 +862,7 @@ bool Environment::setActiveContinuousContactManager(const std::string& name) return setActiveContinuousContactManagerHelper(name); } -tesseract_collision::DiscreteContactManager::UPtr Environment::getDiscreteContactManager() const +std::unique_ptr Environment::getDiscreteContactManager() const { std::shared_lock lock(mutex_); { // Clone cached manager if exists @@ -862,7 +892,7 @@ void Environment::clearCachedDiscreteContactManager() const discrete_manager_ = nullptr; } -tesseract_collision::ContinuousContactManager::UPtr Environment::getContinuousContactManager() const +std::unique_ptr Environment::getContinuousContactManager() const { std::shared_lock lock(mutex_); { // Clone cached manager if exists @@ -892,7 +922,7 @@ void Environment::clearCachedContinuousContactManager() const continuous_manager_ = nullptr; } -tesseract_collision::ContinuousContactManager::UPtr +std::unique_ptr Environment::getContinuousContactManager(const std::string& name) const { std::shared_lock lock(mutex_); @@ -994,7 +1024,7 @@ bool Environment::setActiveContinuousContactManagerHelper(const std::string& nam return true; } -tesseract_collision::DiscreteContactManager::UPtr +std::unique_ptr Environment::getDiscreteContactManagerHelper(const std::string& name) const { tesseract_collision::DiscreteContactManager::UPtr manager = @@ -1026,7 +1056,7 @@ Environment::getDiscreteContactManagerHelper(const std::string& name) const return manager; } -tesseract_collision::ContinuousContactManager::UPtr +std::unique_ptr Environment::getContinuousContactManagerHelper(const std::string& name) const { tesseract_collision::ContinuousContactManager::UPtr manager = @@ -1066,7 +1096,7 @@ Environment::getContinuousContactManagerHelper(const std::string& name) const return manager; } -void Environment::getCollisionObject(tesseract_collision::CollisionShapesConst& shapes, +void Environment::getCollisionObject(std::vector>& shapes, tesseract_common::VectorIsometry3d& shape_poses, const tesseract_scene_graph::Link& link) { @@ -1254,7 +1284,7 @@ Environment::UPtr Environment::clone() const return cloned_env; } -bool Environment::applyCommandsHelper(const Commands& commands) +bool Environment::applyCommandsHelper(const std::vector>& commands) { bool success = true; for (const auto& command : commands) @@ -1548,8 +1578,8 @@ bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand:: return true; } -bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::ConstPtr& link, - const tesseract_scene_graph::Joint::ConstPtr& joint, +bool Environment::applyAddLinkCommandHelper(const std::shared_ptr& link, + const std::shared_ptr& joint, bool replace_allowed) { bool link_exists = false; @@ -1703,7 +1733,7 @@ bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::C return true; } -bool Environment::applyMoveLinkCommand(const MoveLinkCommand::ConstPtr& cmd) +bool Environment::applyMoveLinkCommand(const std::shared_ptr& cmd) { if (!scene_graph_->moveLink(*cmd->getJoint())) return false; @@ -1717,7 +1747,7 @@ bool Environment::applyMoveLinkCommand(const MoveLinkCommand::ConstPtr& cmd) return true; } -bool Environment::applyMoveJointCommand(const MoveJointCommand::ConstPtr& cmd) +bool Environment::applyMoveJointCommand(const std::shared_ptr& cmd) { if (!scene_graph_->moveJoint(cmd->getJointName(), cmd->getParentLink())) return false; @@ -1731,7 +1761,7 @@ bool Environment::applyMoveJointCommand(const MoveJointCommand::ConstPtr& cmd) return true; } -bool Environment::applyRemoveLinkCommand(const RemoveLinkCommand::ConstPtr& cmd) +bool Environment::applyRemoveLinkCommand(const std::shared_ptr& cmd) { if (!removeLinkHelper(cmd->getLinkName())) return false; @@ -1745,7 +1775,7 @@ bool Environment::applyRemoveLinkCommand(const RemoveLinkCommand::ConstPtr& cmd) return true; } -bool Environment::applyRemoveJointCommand(const RemoveJointCommand::ConstPtr& cmd) +bool Environment::applyRemoveJointCommand(const std::shared_ptr& cmd) { if (scene_graph_->getJoint(cmd->getJointName()) == nullptr) { @@ -1767,7 +1797,7 @@ bool Environment::applyRemoveJointCommand(const RemoveJointCommand::ConstPtr& cm return true; } -bool Environment::applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& cmd) +bool Environment::applyReplaceJointCommand(const std::shared_ptr& cmd) { tesseract_scene_graph::Joint::ConstPtr current_joint = scene_graph_->getJoint(cmd->getJoint()->getName()); if (current_joint == nullptr) @@ -1804,7 +1834,7 @@ bool Environment::applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& return true; } -bool Environment::applyChangeLinkOriginCommand(const ChangeLinkOriginCommand::ConstPtr& /*cmd*/) // NOLINT +bool Environment::applyChangeLinkOriginCommand(const std::shared_ptr& /*cmd*/) // NOLINT { throw std::runtime_error("Unhandled environment command: CHANGE_LINK_ORIGIN"); } @@ -1823,7 +1853,8 @@ bool Environment::applyChangeJointOriginCommand(const ChangeJointOriginCommand:: return true; } -bool Environment::applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisionEnabledCommand::ConstPtr& cmd) +bool Environment::applyChangeLinkCollisionEnabledCommand( + const std::shared_ptr& cmd) { std::unique_lock discrete_lock(discrete_manager_mutex_); if (discrete_manager_ != nullptr) @@ -1854,7 +1885,7 @@ bool Environment::applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisi return true; } -bool Environment::applyChangeLinkVisibilityCommand(const ChangeLinkVisibilityCommand::ConstPtr& cmd) +bool Environment::applyChangeLinkVisibilityCommand(const std::shared_ptr& cmd) { scene_graph_->setLinkVisibility(cmd->getLinkName(), cmd->getEnabled()); if (scene_graph_->getLinkVisibility(cmd->getLinkName()) != cmd->getEnabled()) @@ -1866,7 +1897,7 @@ bool Environment::applyChangeLinkVisibilityCommand(const ChangeLinkVisibilityCom return true; } -bool Environment::applyModifyAllowedCollisionsCommand(const ModifyAllowedCollisionsCommand::ConstPtr& cmd) +bool Environment::applyModifyAllowedCollisionsCommand(const std::shared_ptr& cmd) { switch (cmd->getModifyType()) { @@ -1899,7 +1930,8 @@ bool Environment::applyModifyAllowedCollisionsCommand(const ModifyAllowedCollisi return true; } -bool Environment::applyRemoveAllowedCollisionLinkCommand(const RemoveAllowedCollisionLinkCommand::ConstPtr& cmd) +bool Environment::applyRemoveAllowedCollisionLinkCommand( + const std::shared_ptr& cmd) { scene_graph_->removeAllowedCollision(cmd->getLinkName()); @@ -1909,7 +1941,7 @@ bool Environment::applyRemoveAllowedCollisionLinkCommand(const RemoveAllowedColl return true; } -bool Environment::applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd) +bool Environment::applyAddSceneGraphCommand(std::shared_ptr cmd) { if (scene_graph_->isEmpty() && cmd->getJoint()) return false; @@ -1984,7 +2016,8 @@ bool Environment::applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd) return true; } -bool Environment::applyChangeJointPositionLimitsCommand(const ChangeJointPositionLimitsCommand::ConstPtr& cmd) +bool Environment::applyChangeJointPositionLimitsCommand( + const std::shared_ptr& cmd) { // First check if all of the joint exist for (const auto& jp : cmd->getLimits()) @@ -2013,7 +2046,8 @@ bool Environment::applyChangeJointPositionLimitsCommand(const ChangeJointPositio return true; } -bool Environment::applyChangeJointVelocityLimitsCommand(const ChangeJointVelocityLimitsCommand::ConstPtr& cmd) +bool Environment::applyChangeJointVelocityLimitsCommand( + const std::shared_ptr& cmd) { // First check if all of the joint exist for (const auto& jp : cmd->getLimits()) @@ -2041,7 +2075,8 @@ bool Environment::applyChangeJointVelocityLimitsCommand(const ChangeJointVelocit return true; } -bool Environment::applyChangeJointAccelerationLimitsCommand(const ChangeJointAccelerationLimitsCommand::ConstPtr& cmd) +bool Environment::applyChangeJointAccelerationLimitsCommand( + const std::shared_ptr& cmd) { // First check if all of the joint exist for (const auto& jp : cmd->getLimits()) @@ -2069,7 +2104,8 @@ bool Environment::applyChangeJointAccelerationLimitsCommand(const ChangeJointAcc return true; } -bool Environment::applyAddKinematicsInformationCommand(const AddKinematicsInformationCommand::ConstPtr& cmd) +bool Environment::applyAddKinematicsInformationCommand( + const std::shared_ptr& cmd) { kinematics_information_.insert(cmd->getKinematicsInformation()); @@ -2107,7 +2143,8 @@ bool Environment::applyAddKinematicsInformationCommand(const AddKinematicsInform return true; } -bool Environment::applyAddContactManagersPluginInfoCommand(const AddContactManagersPluginInfoCommand::ConstPtr& cmd) +bool Environment::applyAddContactManagersPluginInfoCommand( + const std::shared_ptr& cmd) { const auto& info = cmd->getContactManagersPluginInfo(); @@ -2165,7 +2202,7 @@ bool Environment::applyAddContactManagersPluginInfoCommand(const AddContactManag } bool Environment::applySetActiveContinuousContactManagerCommand( - const SetActiveContinuousContactManagerCommand::ConstPtr& cmd) + const std::shared_ptr& cmd) { setActiveContinuousContactManagerHelper(cmd->getName()); @@ -2176,7 +2213,7 @@ bool Environment::applySetActiveContinuousContactManagerCommand( } bool Environment::applySetActiveDiscreteContactManagerCommand( - const SetActiveDiscreteContactManagerCommand::ConstPtr& cmd) + const std::shared_ptr& cmd) { setActiveDiscreteContactManagerHelper(cmd->getName()); @@ -2186,7 +2223,7 @@ bool Environment::applySetActiveDiscreteContactManagerCommand( return true; } -bool Environment::applyChangeCollisionMarginsCommand(const ChangeCollisionMarginsCommand::ConstPtr& cmd) +bool Environment::applyChangeCollisionMarginsCommand(const std::shared_ptr& cmd) { collision_margin_data_.apply(cmd->getCollisionMarginData(), cmd->getCollisionMarginOverrideType()); diff --git a/tesseract_environment/src/environment_cache.cpp b/tesseract_environment/src/environment_cache.cpp index 201cb0c10c9..0aa998a7635 100644 --- a/tesseract_environment/src/environment_cache.cpp +++ b/tesseract_environment/src/environment_cache.cpp @@ -25,11 +25,24 @@ */ #include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include namespace tesseract_environment { -DefaultEnvironmentCache::DefaultEnvironmentCache(tesseract_environment::Environment::ConstPtr env, - std::size_t cache_size) +DefaultEnvironmentCache::DefaultEnvironmentCache(std::shared_ptr env, std::size_t cache_size) : env_(std::move(env)), cache_size_(cache_size) { } @@ -48,14 +61,14 @@ void DefaultEnvironmentCache::refreshCache() const refreshCacheHelper(); } -tesseract_environment::Environment::UPtr DefaultEnvironmentCache::getCachedEnvironment() const +std::unique_ptr DefaultEnvironmentCache::getCachedEnvironment() const { tesseract_scene_graph::SceneState current_state = env_->getState(); std::unique_lock lock(cache_mutex_); refreshCacheHelper(); // This is to make sure the cached items are updated if needed assert(!cache_.empty()); - tesseract_environment::Environment::UPtr t = std::move(cache_.back()); + std::unique_ptr t = std::move(cache_.back()); // Update to the current joint values t->setState(current_state.joints); @@ -66,7 +79,7 @@ tesseract_environment::Environment::UPtr DefaultEnvironmentCache::getCachedEnvir void DefaultEnvironmentCache::refreshCacheHelper() const { - tesseract_environment::Environment::UPtr env; + std::unique_ptr env; auto lock_read = env_->lockRead(); int rev = env_->getRevision(); if (rev != cache_env_revision_ || cache_.empty()) diff --git a/tesseract_environment/src/environment_monitor.cpp b/tesseract_environment/src/environment_monitor.cpp new file mode 100644 index 00000000000..a3e5584dbf3 --- /dev/null +++ b/tesseract_environment/src/environment_monitor.cpp @@ -0,0 +1,54 @@ +/** + * @file environment_monitor.cpp + * @brief Tesseract Environment Monitor Interface Class. + * + * @author Levi Armstrong + * @date March 30, 2022 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include + +namespace tesseract_environment +{ +EnvironmentMonitor::EnvironmentMonitor(std::string monitor_namespace) : monitor_namespace_(monitor_namespace) {} + +EnvironmentMonitor::EnvironmentMonitor(std::shared_ptr env, std::string monitor_namespace) + : env_(std::move(env)), monitor_namespace_(monitor_namespace) +{ +} + +const std::string& EnvironmentMonitor::getNamespace() const { return monitor_namespace_; } + +Environment& EnvironmentMonitor::environment() { return *env_; } + +const Environment& EnvironmentMonitor::environment() const { return *env_; } + +std::shared_ptr EnvironmentMonitor::getEnvironment() { return env_; } + +std::shared_ptr EnvironmentMonitor::getEnvironment() const { return env_; } +} // namespace tesseract_environment diff --git a/tesseract_environment/src/events.cpp b/tesseract_environment/src/events.cpp new file mode 100644 index 00000000000..29519c889f6 --- /dev/null +++ b/tesseract_environment/src/events.cpp @@ -0,0 +1,44 @@ +/** + * @file events.cpp + * @brief Tesseract Events. + * + * @author Levi Armstrong + * @date March 28, 2022 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +namespace tesseract_environment +{ +Event::Event(Events type) : type(type) {} + +CommandAppliedEvent::CommandAppliedEvent(const std::vector >& commands, int revision) + : Event(Events::COMMAND_APPLIED), commands(commands), revision(revision) +{ +} + +SceneStateChangedEvent::SceneStateChangedEvent(const tesseract_scene_graph::SceneState& state) + : Event(Events::SCENE_STATE_CHANGED), state(state) +{ +} +} // namespace tesseract_environment diff --git a/tesseract_environment/src/utils.cpp b/tesseract_environment/src/utils.cpp index d32598c8a16..a17f77bc7b9 100644 --- a/tesseract_environment/src/utils.cpp +++ b/tesseract_environment/src/utils.cpp @@ -25,11 +25,19 @@ */ #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include +#include +#include +#include +#include + namespace tesseract_environment { /** diff --git a/tesseract_environment/test/tesseract_environment_cache_unit.cpp b/tesseract_environment/test/tesseract_environment_cache_unit.cpp index 3b92da772cc..1c92618c351 100644 --- a/tesseract_environment/test/tesseract_environment_cache_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_cache_unit.cpp @@ -11,6 +11,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include + #include using namespace tesseract_scene_graph; diff --git a/tesseract_environment/test/tesseract_environment_collision.cpp b/tesseract_environment/test/tesseract_environment_collision.cpp index 6ba52583aa8..69fa05edb80 100644 --- a/tesseract_environment/test/tesseract_environment_collision.cpp +++ b/tesseract_environment/test/tesseract_environment_collision.cpp @@ -9,8 +9,24 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +#include +#include +#include + #include using namespace tesseract_scene_graph; diff --git a/tesseract_environment/test/tesseract_environment_serialization_unit.cpp b/tesseract_environment/test/tesseract_environment_serialization_unit.cpp index 852d838f28b..c67f56ad2b4 100644 --- a/tesseract_environment/test/tesseract_environment_serialization_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_serialization_unit.cpp @@ -33,10 +33,27 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include + #include +#include + +#include +#include +#include + +#include + +#include +#include +#include + #include + #include + +#include +#include + #include using namespace tesseract_common; diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp index df470e1f206..e7b51ea97ab 100644 --- a/tesseract_environment/test/tesseract_environment_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_unit.cpp @@ -8,16 +8,34 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include + +#include + #include #include + #include #include +#include + +#include +#include +#include + +#include #include + +#include +#include +#include + #include #include + #include #include #include + #include using namespace tesseract_scene_graph; diff --git a/tesseract_environment/test/tesseract_environment_utils.cpp b/tesseract_environment/test/tesseract_environment_utils.cpp index 1e1e247c796..232846f486d 100644 --- a/tesseract_environment/test/tesseract_environment_utils.cpp +++ b/tesseract_environment/test/tesseract_environment_utils.cpp @@ -10,6 +10,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include + +#include +#include + +#include + +#include + +#include +#include +#include + +#include + #include using namespace tesseract_scene_graph; diff --git a/tesseract_geometry/CMakeLists.txt b/tesseract_geometry/CMakeLists.txt index 7f2b51e718e..d6bb7cbb44e 100644 --- a/tesseract_geometry/CMakeLists.txt +++ b/tesseract_geometry/CMakeLists.txt @@ -70,6 +70,7 @@ add_library( src/geometries/cone.cpp src/geometries/convex_mesh.cpp src/geometries/cylinder.cpp + src/geometries/mesh_material.cpp src/geometries/mesh.cpp src/geometries/octree.cpp src/geometries/plane.cpp diff --git a/tesseract_geometry/examples/create_geometries_example.cpp b/tesseract_geometry/examples/create_geometries_example.cpp index def1cc99503..12583d30d99 100644 --- a/tesseract_geometry/examples/create_geometries_example.cpp +++ b/tesseract_geometry/examples/create_geometries_example.cpp @@ -42,5 +42,5 @@ int main(int /*argc*/, char** /*argv*/) // Create an octree std::shared_ptr octree; - auto octree_t = std::make_shared(octree, tesseract_geometry::Octree::SubType::BOX); + auto octree_t = std::make_shared(octree, tesseract_geometry::OctreeSubType::BOX); } diff --git a/tesseract_geometry/include/tesseract_geometry/fwd.h b/tesseract_geometry/include/tesseract_geometry/fwd.h new file mode 100644 index 00000000000..22f4d224dcf --- /dev/null +++ b/tesseract_geometry/include/tesseract_geometry/fwd.h @@ -0,0 +1,48 @@ +/** + * @file tesseract_geometry_fwd.h + * @brief Tesseract Geometry Forward Declarations + * + * @author Levi Armstrong + * @date January 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_GEOMETRY_TESSERACT_GEOMETRY_FWD_H +#define TESSERACT_GEOMETRY_TESSERACT_GEOMETRY_FWD_H + +namespace tesseract_geometry +{ +enum class GeometryType; +class Geometry; +class Box; +class Capsule; +class Cone; +class ConvexMesh; +class Cylinder; +class MeshMaterial; +class MeshTexture; +class Mesh; +class Octree; +enum class OctreeSubType; +class Plane; +class PolygonMesh; +class SDFMesh; +class Sphere; +} // namespace tesseract_geometry +#endif // TESSERACT_GEOMETRY_TESSERACT_GEOMETRY_FWD_H diff --git a/tesseract_geometry/include/tesseract_geometry/geometry.h b/tesseract_geometry/include/tesseract_geometry/geometry.h index 62da90dfef2..a44ad367450 100644 --- a/tesseract_geometry/include/tesseract_geometry/geometry.h +++ b/tesseract_geometry/include/tesseract_geometry/geometry.h @@ -37,7 +37,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { -enum GeometryType +enum class GeometryType { UNINITIALIZED, SPHERE, diff --git a/tesseract_geometry/include/tesseract_geometry/impl/box.h b/tesseract_geometry/include/tesseract_geometry/impl/box.h index 61e7f190ab5..ff3e42dca31 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/box.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/box.h @@ -43,15 +43,15 @@ class Box : public Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Box(double x, double y, double z) : Geometry(GeometryType::BOX), x_(x), y_(y), z_(z) {} + Box(double x, double y, double z); Box() = default; ~Box() override = default; - double getX() const { return x_; } - double getY() const { return y_; } - double getZ() const { return z_; } + double getX() const; + double getY() const; + double getZ() const; - Geometry::Ptr clone() const override final { return std::make_shared(x_, y_, z_); } + Geometry::Ptr clone() const override final; bool operator==(const Box& rhs) const; bool operator!=(const Box& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/capsule.h b/tesseract_geometry/include/tesseract_geometry/impl/capsule.h index aa338281d88..e7a45b2a7ad 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/capsule.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/capsule.h @@ -43,14 +43,14 @@ class Capsule : public Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Capsule(double r, double l) : Geometry(GeometryType::CAPSULE), r_(r), l_(l) {} + Capsule(double r, double l); Capsule() = default; ~Capsule() override = default; - double getRadius() const { return r_; } - double getLength() const { return l_; } + double getRadius() const; + double getLength() const; - Geometry::Ptr clone() const override final { return std::make_shared(r_, l_); } + Geometry::Ptr clone() const override final; bool operator==(const Capsule& rhs) const; bool operator!=(const Capsule& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/cone.h b/tesseract_geometry/include/tesseract_geometry/impl/cone.h index e1e7e481204..77ad4239e8d 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/cone.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/cone.h @@ -43,14 +43,14 @@ class Cone : public Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Cone(double r, double l) : Geometry(GeometryType::CONE), r_(r), l_(l) {} + Cone(double r, double l); Cone() = default; ~Cone() override = default; - double getRadius() const { return r_; } - double getLength() const { return l_; } + double getRadius() const; + double getLength() const; - Geometry::Ptr clone() const override final { return std::make_shared(r_, l_); } + Geometry::Ptr clone() const override final; bool operator==(const Cone& rhs) const; bool operator!=(const Cone& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/convex_mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/convex_mesh.h index 3f0b02bcead..b012af31680 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/convex_mesh.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/convex_mesh.h @@ -34,8 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include #include namespace tesseract_geometry @@ -74,23 +72,12 @@ class ConvexMesh : public PolygonMesh */ ConvexMesh(std::shared_ptr vertices, std::shared_ptr faces, - tesseract_common::Resource::ConstPtr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr) - : PolygonMesh(std::move(vertices), - std::move(faces), - std::move(resource), - scale, - std::move(normals), - std::move(vertex_colors), - std::move(mesh_material), - std::move(mesh_textures), - GeometryType::CONVEX_MESH) - { - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr); /** * @brief Convex Mesh geometry @@ -112,24 +99,12 @@ class ConvexMesh : public PolygonMesh ConvexMesh(std::shared_ptr vertices, std::shared_ptr faces, int face_count, - tesseract_common::Resource::ConstPtr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr) - : PolygonMesh(std::move(vertices), - std::move(faces), - face_count, - std::move(resource), - scale, - std::move(normals), - std::move(vertex_colors), - std::move(mesh_material), - std::move(mesh_textures), - GeometryType::CONVEX_MESH) - { - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr); ConvexMesh() = default; ~ConvexMesh() override = default; @@ -139,19 +114,17 @@ class ConvexMesh : public PolygonMesh * @note This used when writing back out to urdf * @return The CreationMethod */ - CreationMethod getCreationMethod() const { return creation_method_; } + CreationMethod getCreationMethod() const; /** * @brief Set the method used to create the convex mesh * @note This used when writing back out to urdf * @param value The CreationMethod */ - void setCreationMethod(CreationMethod method) { creation_method_ = method; } + void setCreationMethod(CreationMethod method); + + Geometry::Ptr clone() const override; - Geometry::Ptr clone() const override - { - return std::make_shared(getVertices(), getFaces(), getFaceCount(), getResource(), getScale()); - } bool operator==(const ConvexMesh& rhs) const; bool operator!=(const ConvexMesh& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/cylinder.h b/tesseract_geometry/include/tesseract_geometry/impl/cylinder.h index c564569d4d6..b3b1df34b99 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/cylinder.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/cylinder.h @@ -43,14 +43,14 @@ class Cylinder : public Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Cylinder(double r, double l) : Geometry(GeometryType::CYLINDER), r_(r), l_(l) {} + Cylinder(double r, double l); Cylinder() = default; ~Cylinder() override = default; - double getRadius() const { return r_; } - double getLength() const { return l_; } + double getRadius() const; + double getLength() const; - Geometry::Ptr clone() const override final { return std::make_shared(r_, l_); } + Geometry::Ptr clone() const override final; bool operator==(const Cylinder& rhs) const; bool operator!=(const Cylinder& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/mesh.h index 2b9558b6cc5..1acb85ae22d 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/mesh.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/mesh.h @@ -27,7 +27,6 @@ #define TESSERACT_GEOMETRY_MESH_H #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include @@ -35,9 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include #include namespace tesseract_geometry @@ -68,26 +64,12 @@ class Mesh : public PolygonMesh */ Mesh(std::shared_ptr vertices, std::shared_ptr triangles, - tesseract_common::Resource::Ptr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr) - : PolygonMesh(std::move(vertices), - std::move(triangles), - std::move(resource), - scale, - std::move(normals), - std::move(vertex_colors), - std::move(mesh_material), - std::move(mesh_textures), - GeometryType::MESH) - { - if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE - } - + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr); /** * @brief Mesh geometry * @param vertices A vector of vertices associated with the mesh @@ -107,60 +89,18 @@ class Mesh : public PolygonMesh Mesh(std::shared_ptr vertices, std::shared_ptr triangles, int triangle_count, - tesseract_common::Resource::ConstPtr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr) - : PolygonMesh(std::move(vertices), - std::move(triangles), - triangle_count, - std::move(resource), - scale, - std::move(normals), - std::move(vertex_colors), - std::move(mesh_material), - std::move(mesh_textures), - GeometryType::MESH) - { - if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr); Mesh() = default; ~Mesh() override = default; - Geometry::Ptr clone() const override - { - // getMaterial returns a pointer-to-const, so deference and make_shared, but also guard against nullptr - std::shared_ptr ptr; - if (getMaterial() != nullptr) - { - ptr = std::make_shared(getVertices(), - getFaces(), - getFaceCount(), - getResource(), - getScale(), - getNormals(), - getVertexColors(), - std::make_shared(*getMaterial()), - getTextures()); - } - else - { - ptr = std::make_shared(getVertices(), - getFaces(), - getFaceCount(), - getResource(), - getScale(), - getNormals(), - getVertexColors(), - nullptr, - getTextures()); - } - return ptr; - } + Geometry::Ptr clone() const override final; + bool operator==(const Mesh& rhs) const; bool operator!=(const Mesh& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/mesh_material.h b/tesseract_geometry/include/tesseract_geometry/impl/mesh_material.h index 7d18ad83598..7a0cc3baa2d 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/mesh_material.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/mesh_material.h @@ -27,14 +27,14 @@ #define TESSERACT_GEOMETRY_MESH_MATERIAL_H #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include +#include namespace tesseract_geometry { @@ -60,27 +60,26 @@ namespace tesseract_geometry class MeshMaterial { public: + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; /** * @brief Construct a new MeshMaterial * - * @param baseColorFactor The base color of the mesh - * @param metallicFactor The metallic factor parameter (PBR parameter) - * @param roughnessFactor The roughness factor parameter (PBR parameter) - * @param emissiveFactor The emissivity of the mesh + * @param base_color_factor The base color of the mesh + * @param metallic_factor The metallic factor parameter (PBR parameter) + * @param roughness_factor The roughness factor parameter (PBR parameter) + * @param emissive_factor The emissivity of the mesh */ - MeshMaterial(Eigen::Vector4d baseColorFactor, - double metallicFactor, - double roughnessFactor, - Eigen::Vector4d emissiveFactor) - : base_color_factor_(std::move(baseColorFactor)) - , metallic_factor_(metallicFactor) - , roughness_factor_(roughnessFactor) - , emissive_factor_(std::move(emissiveFactor)) - { - } + MeshMaterial(const Eigen::Vector4d& base_color_factor, + double metallic_factor, + double roughness_factor, + const Eigen::Vector4d& emissive_factor); + MeshMaterial() = default; /** @@ -88,21 +87,21 @@ class MeshMaterial * * @return The base color in RGBA */ - Eigen::Vector4d getBaseColorFactor() const { return base_color_factor_; } + Eigen::Vector4d getBaseColorFactor() const; /** * @brief Get the Metallic Factor of the mesh (PBR parameter) * * @return The metallic factor, between 0 and 1 */ - double getMetallicFactor() const { return metallic_factor_; } + double getMetallicFactor() const; /** * @brief Get the Roughness Factor of the mesh (PBR parameter) * * @return The roughness factor, between 0 and 1 */ - double getRoughnessFactor() const { return roughness_factor_; } + double getRoughnessFactor() const; /** * @brief Get the emissive factor of the mesh @@ -112,7 +111,7 @@ class MeshMaterial * * @return The emissive factor in RGBA */ - Eigen::Vector4d getEmissiveFactor() const { return emissive_factor_; } + Eigen::Vector4d getEmissiveFactor() const; private: // Mesh material based on simplified glTF 2.0 pbrMetallicRoughness parameters @@ -147,11 +146,8 @@ class MeshTexture * @param texture_image Resource representing the texture image (jpg or png) * @param uvs UV coordinates for texture on mesh */ - MeshTexture(tesseract_common::Resource::Ptr texture_image, - std::shared_ptr uvs) - : uvs_(std::move(uvs)), texture_image_(std::move(texture_image)) - { - } + MeshTexture(std::shared_ptr texture_image, + std::shared_ptr uvs); /** * @brief Get the texture image @@ -160,20 +156,20 @@ class MeshTexture * * @return Resource to the texture image */ - tesseract_common::Resource::Ptr getTextureImage() const { return texture_image_; } + std::shared_ptr getTextureImage() const; /** * @brief Get the texture UV coordinates * * @return UV coordinate vector */ - const std::shared_ptr& getUVs() { return uvs_; } + const std::shared_ptr& getUVs(); private: std::shared_ptr uvs_; // texture_image shall be jpg or png - tesseract_common::Resource::Ptr texture_image_; + std::shared_ptr texture_image_; }; } // namespace tesseract_geometry diff --git a/tesseract_geometry/include/tesseract_geometry/impl/octree.h b/tesseract_geometry/include/tesseract_geometry/impl/octree.h index c7f0d39468b..ccf101c0bd0 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/octree.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/octree.h @@ -32,35 +32,37 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +namespace octomap +{ +class OcTree; +class OcTreeNode; +} // namespace octomap + namespace tesseract_geometry { +enum class OctreeSubType +{ + BOX, + SPHERE_INSIDE, + SPHERE_OUTSIDE +}; + class Octree : public Geometry { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - enum SubType - { - BOX, - SPHERE_INSIDE, - SPHERE_OUTSIDE - }; - - Octree(std::shared_ptr octree, const SubType sub_type) - : Geometry(GeometryType::OCTREE), octree_(std::move(octree)), sub_type_(sub_type) - { - } + Octree(std::shared_ptr octree, const OctreeSubType sub_type); template Octree(const PointT& point_cloud, const double resolution, - const SubType sub_type, + const OctreeSubType sub_type, const bool prune, const bool binary = true) : Geometry(GeometryType::OCTREE), sub_type_(sub_type), resolution_(resolution) @@ -91,13 +93,14 @@ class Octree : public Geometry Octree() = default; ~Octree() override = default; - const std::shared_ptr& getOctree() const { return octree_; } + const std::shared_ptr& getOctree() const; + + OctreeSubType getSubType() const; - SubType getSubType() const { return sub_type_; } + bool getPruned() const; - bool getPruned() const { return pruned_; } + Geometry::Ptr clone() const override final; - Geometry::Ptr clone() const override final { return std::make_shared(octree_, sub_type_); } bool operator==(const Octree& rhs) const; bool operator!=(const Octree& rhs) const; @@ -114,99 +117,25 @@ class Octree : public Geometry * * @return number of sub shapes */ - long calcNumSubShapes() const - { - long cnt = 0; - double occupancy_threshold = octree_->getOccupancyThres(); - for (auto it = octree_->begin(static_cast(octree_->getTreeDepth())), end = octree_->end(); it != end; - ++it) - if (it->getOccupancy() >= occupancy_threshold) - ++cnt; - - return cnt; - } + long calcNumSubShapes() const; private: std::shared_ptr octree_; - SubType sub_type_{ SubType::BOX }; + OctreeSubType sub_type_{ OctreeSubType::BOX }; double resolution_{ 0.01 }; bool pruned_{ false }; bool binary_octree_{ false }; - static bool isNodeCollapsible(octomap::OcTree& octree, octomap::OcTreeNode* node) - { - if (!octree.nodeChildExists(node, 0)) - return false; - - double occupancy_threshold = octree.getOccupancyThres(); - - const octomap::OcTreeNode* firstChild = octree.getNodeChild(node, 0); - if (octree.nodeHasChildren(firstChild) || firstChild->getOccupancy() < occupancy_threshold) - return false; - - for (unsigned int i = 1; i < 8; i++) - { - // comparison via getChild so that casts of derived classes ensure - // that the right == operator gets called - if (!octree.nodeChildExists(node, i)) - return false; - - if (octree.nodeHasChildren(octree.getNodeChild(node, i))) - return false; - - if (octree.getNodeChild(node, i)->getOccupancy() < occupancy_threshold) - return false; - } - - return true; - } - - static bool pruneNode(octomap::OcTree& octree, octomap::OcTreeNode* node) - { - if (!isNodeCollapsible(octree, node)) - return false; - - // set value to children's values (all assumed equal) - node->copyData(*(octree.getNodeChild(node, 0))); + static bool isNodeCollapsible(octomap::OcTree& octree, octomap::OcTreeNode* node); - // delete children (known to be leafs at this point!) - for (unsigned int i = 0; i < 8; i++) - { - octree.deleteNodeChild(node, i); - } - - return true; - } + static bool pruneNode(octomap::OcTree& octree, octomap::OcTreeNode* node); // NOLINTNEXTLINE(misc-no-recursion) static void pruneRecurs(octomap::OcTree& octree, octomap::OcTreeNode* node, unsigned int depth, unsigned int max_depth, - unsigned int& num_pruned) - { - assert(node); - - if (depth < max_depth) - { - for (unsigned int i = 0; i < 8; i++) - { - if (octree.nodeChildExists(node, i)) - { - pruneRecurs(octree, octree.getNodeChild(node, i), depth + 1, max_depth, num_pruned); - } - } - } // end if depth - - else - { - // max level reached - if (pruneNode(octree, node)) - { - num_pruned++; - } - } - } + unsigned int& num_pruned); friend class boost::serialization::access; template @@ -227,19 +156,7 @@ class Octree : public Geometry * * @param octree The octree to be pruned. */ - static void prune(octomap::OcTree& octree) - { - if (octree.getRoot() == nullptr) - return; - - for (unsigned int depth = octree.getTreeDepth() - 1; depth > 0; --depth) - { - unsigned int num_pruned = 0; - pruneRecurs(octree, octree.getRoot(), 0, depth, num_pruned); - if (num_pruned == 0) - break; - } - } + static void prune(octomap::OcTree& octree); }; } // namespace tesseract_geometry diff --git a/tesseract_geometry/include/tesseract_geometry/impl/plane.h b/tesseract_geometry/include/tesseract_geometry/impl/plane.h index 596f7c4cd48..cae47126f50 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/plane.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/plane.h @@ -43,16 +43,16 @@ class Plane : public Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Plane(double a, double b, double c, double d) : Geometry(GeometryType::PLANE), a_(a), b_(b), c_(c), d_(d) {} + Plane(double a, double b, double c, double d); Plane() = default; ~Plane() override = default; - double getA() const { return a_; } - double getB() const { return b_; } - double getC() const { return c_; } - double getD() const { return d_; } + double getA() const; + double getB() const; + double getC() const; + double getD() const; - Geometry::Ptr clone() const override final { return std::make_shared(a_, b_, c_, d_); } + Geometry::Ptr clone() const override final; bool operator==(const Plane& rhs) const; bool operator!=(const Plane& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/polygon_mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/polygon_mesh.h index ec8a5eee9f3..8060d8eb1a3 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/polygon_mesh.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/polygon_mesh.h @@ -34,14 +34,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include + #include -#include namespace tesseract_geometry { +class MeshMaterial; +class MeshTexture; + class PolygonMesh : public Geometry { public: @@ -69,31 +71,13 @@ class PolygonMesh : public Geometry */ PolygonMesh(std::shared_ptr vertices, std::shared_ptr faces, - tesseract_common::Resource::ConstPtr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), // NOLINT std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr, - GeometryType type = GeometryType::POLYGON_MESH) - : Geometry(type) - , vertices_(std::move(vertices)) - , faces_(std::move(faces)) - , vertex_count_(static_cast(vertices_->size())) - , resource_(std::move(resource)) - , scale_(scale) - , normals_(std::move(normals)) - , vertex_colors_(std::move(vertex_colors)) - , mesh_material_(std::move(mesh_material)) - , mesh_textures_(std::move(mesh_textures)) - { - for (int i = 0; i < faces_->size(); ++i) - { - ++face_count_; - int num_verts = (*faces_)(i); // NOLINT - i += num_verts; - } - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr, + GeometryType type = GeometryType::POLYGON_MESH); /** * @brief Polygon Mesh geometry @@ -115,26 +99,13 @@ class PolygonMesh : public Geometry PolygonMesh(std::shared_ptr vertices, std::shared_ptr faces, int face_count, - tesseract_common::Resource::ConstPtr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), // NOLINT std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr, - GeometryType type = GeometryType::POLYGON_MESH) - : Geometry(type) - , vertices_(std::move(vertices)) - , faces_(std::move(faces)) - , vertex_count_(static_cast(vertices_->size())) - , face_count_(face_count) - , resource_(std::move(resource)) - , scale_(scale) - , normals_(std::move(normals)) - , vertex_colors_(std::move(vertex_colors)) - , mesh_material_(std::move(mesh_material)) - , mesh_textures_(std::move(mesh_textures)) - { - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr, + GeometryType type = GeometryType::POLYGON_MESH); PolygonMesh() = default; ~PolygonMesh() override = default; @@ -143,25 +114,25 @@ class PolygonMesh : public Geometry * @brief Get Polygon mesh vertices * @return A vector of vertices */ - const std::shared_ptr& getVertices() const { return vertices_; } + const std::shared_ptr& getVertices() const; /** * @brief Get Polygon mesh faces * @return A vector of face indices */ - const std::shared_ptr& getFaces() const { return faces_; } + const std::shared_ptr& getFaces() const; /** * @brief Get vertex count * @return Number of vertices */ - int getVertexCount() const { return vertex_count_; } + int getVertexCount() const; /** * @brief Get face count * @return Number of faces */ - int getFaceCount() const { return face_count_; } + int getFaceCount() const; /** * @brief Get the path to file used to generate the mesh @@ -170,13 +141,13 @@ class PolygonMesh : public Geometry * * @return Absolute path to the mesh file */ - tesseract_common::Resource::ConstPtr getResource() const { return resource_; } + std::shared_ptr getResource() const; /** * @brief Get the scale applied to file used to generate the mesh * @return The scale x, y, z */ - const Eigen::Vector3d& getScale() const { return scale_; } + const Eigen::Vector3d& getScale() const; /** * @brief Get the vertex normal vectors @@ -185,7 +156,7 @@ class PolygonMesh : public Geometry * * @return The vertex normal vector */ - const std::shared_ptr& getNormals() const { return normals_; } + const std::shared_ptr& getNormals() const; /** * @brief Get the vertex colors @@ -194,7 +165,7 @@ class PolygonMesh : public Geometry * * @return Vertex colors */ - const std::shared_ptr& getVertexColors() const { return vertex_colors_; } + const std::shared_ptr& getVertexColors() const; /** * @brief Get material data extracted from the mesh file @@ -204,7 +175,7 @@ class PolygonMesh : public Geometry * * @return The MeshMaterial data extracted from mesh file */ - MeshMaterial::ConstPtr getMaterial() const { return mesh_material_; } + std::shared_ptr getMaterial() const; /** * @brief Get textures extracted from the mesh file @@ -216,12 +187,10 @@ class PolygonMesh : public Geometry * * @return Vector of mesh textures */ - const std::shared_ptr>& getTextures() const { return mesh_textures_; } + const std::shared_ptr>>& getTextures() const; + + Geometry::Ptr clone() const override; - Geometry::Ptr clone() const override - { - return std::make_shared(vertices_, faces_, face_count_, resource_, scale_); - } bool operator==(const PolygonMesh& rhs) const; bool operator!=(const PolygonMesh& rhs) const; @@ -231,12 +200,12 @@ class PolygonMesh : public Geometry int vertex_count_{ 0 }; int face_count_{ 0 }; - tesseract_common::Resource::ConstPtr resource_; + std::shared_ptr resource_; Eigen::Vector3d scale_; std::shared_ptr normals_; std::shared_ptr vertex_colors_; - MeshMaterial::Ptr mesh_material_; - std::shared_ptr> mesh_textures_; + std::shared_ptr mesh_material_; + std::shared_ptr>> mesh_textures_; friend class boost::serialization::access; template diff --git a/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h index 9b6e13b580f..ebd7542e38f 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h @@ -34,8 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include #include namespace tesseract_geometry @@ -66,25 +64,12 @@ class SDFMesh : public PolygonMesh */ SDFMesh(std::shared_ptr vertices, std::shared_ptr triangles, - tesseract_common::Resource::Ptr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr) - : PolygonMesh(std::move(vertices), - std::move(triangles), - std::move(resource), - scale, - std::move(normals), - std::move(vertex_colors), - std::move(mesh_material), - std::move(mesh_textures), - GeometryType::SDF_MESH) - { - if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr); /** * @brief SDF Mesh geometry @@ -104,34 +89,17 @@ class SDFMesh : public PolygonMesh SDFMesh(std::shared_ptr vertices, std::shared_ptr triangles, int triangle_count, - tesseract_common::Resource::ConstPtr resource = nullptr, + std::shared_ptr resource = nullptr, const Eigen::Vector3d& scale = Eigen::Vector3d(1, 1, 1), std::shared_ptr normals = nullptr, std::shared_ptr vertex_colors = nullptr, - MeshMaterial::Ptr mesh_material = nullptr, - std::shared_ptr> mesh_textures = nullptr) - : PolygonMesh(std::move(vertices), - std::move(triangles), - triangle_count, - std::move(resource), - scale, - std::move(normals), - std::move(vertex_colors), - std::move(mesh_material), - std::move(mesh_textures), - GeometryType::SDF_MESH) - { - if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE - } + std::shared_ptr mesh_material = nullptr, + std::shared_ptr>> mesh_textures = nullptr); SDFMesh() = default; ~SDFMesh() override = default; - Geometry::Ptr clone() const override - { - return std::make_shared(getVertices(), getFaces(), getFaceCount(), getResource(), getScale()); - } + Geometry::Ptr clone() const override final; bool operator==(const SDFMesh& rhs) const; bool operator!=(const SDFMesh& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/sphere.h b/tesseract_geometry/include/tesseract_geometry/impl/sphere.h index 355759d1419..46b64d64db9 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/sphere.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/sphere.h @@ -43,13 +43,13 @@ class Sphere : public Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - explicit Sphere(double r) : Geometry(GeometryType::SPHERE), r_(r) {} + explicit Sphere(double r); Sphere() = default; ~Sphere() override = default; - double getRadius() const { return r_; } + double getRadius() const; - Geometry::Ptr clone() const override final { return std::make_shared(r_); } + Geometry::Ptr clone() const override final; bool operator==(const Sphere& rhs) const; bool operator!=(const Sphere& rhs) const; diff --git a/tesseract_geometry/include/tesseract_geometry/utils.h b/tesseract_geometry/include/tesseract_geometry/utils.h index 0bef759ede6..1c56f61941e 100644 --- a/tesseract_geometry/include/tesseract_geometry/utils.h +++ b/tesseract_geometry/include/tesseract_geometry/utils.h @@ -26,10 +26,10 @@ #ifndef TESSERACT_GEOMETRY_UTILS_H #define TESSERACT_GEOMETRY_UTILS_H -#include - namespace tesseract_geometry { +class Geometry; + /** * @brief Check if two Geometries are identical * @param geom1 First Geometry diff --git a/tesseract_geometry/src/geometries/box.cpp b/tesseract_geometry/src/geometries/box.cpp index 320ad5974d3..7a255298e1d 100644 --- a/tesseract_geometry/src/geometries/box.cpp +++ b/tesseract_geometry/src/geometries/box.cpp @@ -34,6 +34,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Box::Box(double x, double y, double z) : Geometry(GeometryType::BOX), x_(x), y_(y), z_(z) {} + +double Box::getX() const { return x_; } +double Box::getY() const { return y_; } +double Box::getZ() const { return z_; } + +Geometry::Ptr Box::clone() const { return std::make_shared(x_, y_, z_); } + bool Box::operator==(const Box& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/capsule.cpp b/tesseract_geometry/src/geometries/capsule.cpp index ee3204fecc7..f5d250a9b35 100644 --- a/tesseract_geometry/src/geometries/capsule.cpp +++ b/tesseract_geometry/src/geometries/capsule.cpp @@ -34,6 +34,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Capsule::Capsule(double r, double l) : Geometry(GeometryType::CAPSULE), r_(r), l_(l) {} + +double Capsule::getRadius() const { return r_; } +double Capsule::getLength() const { return l_; } + +Geometry::Ptr Capsule::clone() const { return std::make_shared(r_, l_); } + bool Capsule::operator==(const Capsule& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/cone.cpp b/tesseract_geometry/src/geometries/cone.cpp index 634e398a9b0..f73bf65c9df 100644 --- a/tesseract_geometry/src/geometries/cone.cpp +++ b/tesseract_geometry/src/geometries/cone.cpp @@ -34,6 +34,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Cone::Cone(double r, double l) : Geometry(GeometryType::CONE), r_(r), l_(l) {} + +double Cone::getRadius() const { return r_; } +double Cone::getLength() const { return l_; } + +Geometry::Ptr Cone::clone() const { return std::make_shared(r_, l_); } + bool Cone::operator==(const Cone& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/convex_mesh.cpp b/tesseract_geometry/src/geometries/convex_mesh.cpp index 3db89c3d6d3..1b91d6dbeea 100644 --- a/tesseract_geometry/src/geometries/convex_mesh.cpp +++ b/tesseract_geometry/src/geometries/convex_mesh.cpp @@ -29,11 +29,63 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tesseract_geometry { +ConvexMesh::ConvexMesh(std::shared_ptr vertices, + std::shared_ptr faces, + tesseract_common::Resource::ConstPtr resource, + const Eigen::Vector3d& scale, + std::shared_ptr normals, + std::shared_ptr vertex_colors, + MeshMaterial::Ptr mesh_material, + std::shared_ptr> mesh_textures) + : PolygonMesh(std::move(vertices), + std::move(faces), + std::move(resource), + scale, + std::move(normals), + std::move(vertex_colors), + std::move(mesh_material), + std::move(mesh_textures), + GeometryType::CONVEX_MESH) +{ +} + +ConvexMesh::ConvexMesh(std::shared_ptr vertices, + std::shared_ptr faces, + int face_count, + tesseract_common::Resource::ConstPtr resource, + const Eigen::Vector3d& scale, + std::shared_ptr normals, + std::shared_ptr vertex_colors, + MeshMaterial::Ptr mesh_material, + std::shared_ptr> mesh_textures) + : PolygonMesh(std::move(vertices), + std::move(faces), + face_count, + std::move(resource), + scale, + std::move(normals), + std::move(vertex_colors), + std::move(mesh_material), + std::move(mesh_textures), + GeometryType::CONVEX_MESH) +{ +} + +ConvexMesh::CreationMethod ConvexMesh::getCreationMethod() const { return creation_method_; } + +void ConvexMesh::setCreationMethod(CreationMethod method) { creation_method_ = method; } + +Geometry::Ptr ConvexMesh::clone() const +{ + return std::make_shared(getVertices(), getFaces(), getFaceCount(), getResource(), getScale()); +} + bool ConvexMesh::operator==(const ConvexMesh& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/cylinder.cpp b/tesseract_geometry/src/geometries/cylinder.cpp index 04ccc120871..210c5468cb6 100644 --- a/tesseract_geometry/src/geometries/cylinder.cpp +++ b/tesseract_geometry/src/geometries/cylinder.cpp @@ -34,6 +34,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Cylinder::Cylinder(double r, double l) : Geometry(GeometryType::CYLINDER), r_(r), l_(l) {} + +double Cylinder::getRadius() const { return r_; } +double Cylinder::getLength() const { return l_; } + +Geometry::Ptr Cylinder::clone() const { return std::make_shared(r_, l_); } + bool Cylinder::operator==(const Cylinder& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/mesh.cpp b/tesseract_geometry/src/geometries/mesh.cpp index 42a63795383..80d23d14324 100644 --- a/tesseract_geometry/src/geometries/mesh.cpp +++ b/tesseract_geometry/src/geometries/mesh.cpp @@ -29,11 +29,89 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tesseract_geometry { +Mesh::Mesh(std::shared_ptr vertices, + std::shared_ptr triangles, + std::shared_ptr resource, + const Eigen::Vector3d& scale, + std::shared_ptr normals, + std::shared_ptr vertex_colors, + std::shared_ptr mesh_material, + std::shared_ptr>> mesh_textures) + : PolygonMesh(std::move(vertices), + std::move(triangles), + std::move(resource), + scale, + std::move(normals), + std::move(vertex_colors), + std::move(mesh_material), + std::move(mesh_textures), + GeometryType::MESH) +{ + if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE +} + +Mesh::Mesh(std::shared_ptr vertices, + std::shared_ptr triangles, + int triangle_count, + std::shared_ptr resource, + const Eigen::Vector3d& scale, + std::shared_ptr normals, + std::shared_ptr vertex_colors, + std::shared_ptr mesh_material, + std::shared_ptr>> mesh_textures) + : PolygonMesh(std::move(vertices), + std::move(triangles), + triangle_count, + std::move(resource), + scale, + std::move(normals), + std::move(vertex_colors), + std::move(mesh_material), + std::move(mesh_textures), + GeometryType::MESH) +{ + if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE +} + +Geometry::Ptr Mesh::clone() const +{ + // getMaterial returns a pointer-to-const, so deference and make_shared, but also guard against nullptr + std::shared_ptr ptr; + if (getMaterial() != nullptr) + { + ptr = std::make_shared(getVertices(), + getFaces(), + getFaceCount(), + getResource(), + getScale(), + getNormals(), + getVertexColors(), + std::make_shared(*getMaterial()), + getTextures()); + } + else + { + ptr = std::make_shared(getVertices(), + getFaces(), + getFaceCount(), + getResource(), + getScale(), + getNormals(), + getVertexColors(), + nullptr, + getTextures()); + } + return ptr; +} + bool Mesh::operator==(const Mesh& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/mesh_material.cpp b/tesseract_geometry/src/geometries/mesh_material.cpp new file mode 100644 index 00000000000..e614c0d7090 --- /dev/null +++ b/tesseract_geometry/src/geometries/mesh_material.cpp @@ -0,0 +1,61 @@ +/** + * @file mesh_material.cpp + * @brief Tesseract Mesh Material read from a mesh file + * + * @author John Wason + * @date December 21, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Wason Technology, LLC + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +namespace tesseract_geometry +{ +MeshMaterial::MeshMaterial(const Eigen::Vector4d& base_color_factor, + double metallic_factor, + double roughness_factor, + const Eigen::Vector4d& emissive_factor) + : base_color_factor_(base_color_factor) + , metallic_factor_(metallic_factor) + , roughness_factor_(roughness_factor) + , emissive_factor_(emissive_factor) +{ +} + +Eigen::Vector4d MeshMaterial::getBaseColorFactor() const { return base_color_factor_; } + +double MeshMaterial::getMetallicFactor() const { return metallic_factor_; } + +double MeshMaterial::getRoughnessFactor() const { return roughness_factor_; } + +Eigen::Vector4d MeshMaterial::getEmissiveFactor() const { return emissive_factor_; } + +MeshTexture::MeshTexture(std::shared_ptr texture_image, + std::shared_ptr uvs) + : uvs_(std::move(uvs)), texture_image_(std::move(texture_image)) +{ +} + +std::shared_ptr MeshTexture::getTextureImage() const { return texture_image_; } + +const std::shared_ptr& MeshTexture::getUVs() { return uvs_; } + +} // namespace tesseract_geometry diff --git a/tesseract_geometry/src/geometries/octree.cpp b/tesseract_geometry/src/geometries/octree.cpp index 14e6ea136de..83dd781b127 100644 --- a/tesseract_geometry/src/geometries/octree.cpp +++ b/tesseract_geometry/src/geometries/octree.cpp @@ -24,12 +24,11 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include #include #include #include -#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -37,6 +36,120 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Octree::Octree(std::shared_ptr octree, const OctreeSubType sub_type) + : Geometry(GeometryType::OCTREE), octree_(std::move(octree)), sub_type_(sub_type) +{ +} + +const std::shared_ptr& Octree::getOctree() const { return octree_; } + +OctreeSubType Octree::getSubType() const { return sub_type_; } + +bool Octree::getPruned() const { return pruned_; } + +Geometry::Ptr Octree::clone() const { return std::make_shared(octree_, sub_type_); } + +long Octree::calcNumSubShapes() const +{ + long cnt = 0; + double occupancy_threshold = octree_->getOccupancyThres(); + for (auto it = octree_->begin(static_cast(octree_->getTreeDepth())), end = octree_->end(); it != end; + ++it) + if (it->getOccupancy() >= occupancy_threshold) + ++cnt; + + return cnt; +} + +bool Octree::isNodeCollapsible(octomap::OcTree& octree, octomap::OcTreeNode* node) +{ + if (!octree.nodeChildExists(node, 0)) + return false; + + double occupancy_threshold = octree.getOccupancyThres(); + + const octomap::OcTreeNode* firstChild = octree.getNodeChild(node, 0); + if (octree.nodeHasChildren(firstChild) || firstChild->getOccupancy() < occupancy_threshold) + return false; + + for (unsigned int i = 1; i < 8; i++) + { + // comparison via getChild so that casts of derived classes ensure + // that the right == operator gets called + if (!octree.nodeChildExists(node, i)) + return false; + + if (octree.nodeHasChildren(octree.getNodeChild(node, i))) + return false; + + if (octree.getNodeChild(node, i)->getOccupancy() < occupancy_threshold) + return false; + } + + return true; +} + +bool Octree::pruneNode(octomap::OcTree& octree, octomap::OcTreeNode* node) +{ + if (!isNodeCollapsible(octree, node)) + return false; + + // set value to children's values (all assumed equal) + node->copyData(*(octree.getNodeChild(node, 0))); + + // delete children (known to be leafs at this point!) + for (unsigned int i = 0; i < 8; i++) + { + octree.deleteNodeChild(node, i); + } + + return true; +} + +// NOLINTNEXTLINE(misc-no-recursion) +void Octree::pruneRecurs(octomap::OcTree& octree, + octomap::OcTreeNode* node, + unsigned int depth, + unsigned int max_depth, + unsigned int& num_pruned) +{ + assert(node); + + if (depth < max_depth) + { + for (unsigned int i = 0; i < 8; i++) + { + if (octree.nodeChildExists(node, i)) + { + pruneRecurs(octree, octree.getNodeChild(node, i), depth + 1, max_depth, num_pruned); + } + } + } // end if depth + + else + { + // max level reached + if (pruneNode(octree, node)) + { + num_pruned++; + } + } +} + +void Octree::prune(octomap::OcTree& octree) +{ + if (octree.getRoot() == nullptr) + return; + + for (unsigned int depth = octree.getTreeDepth() - 1; depth > 0; --depth) + { + unsigned int num_pruned = 0; + pruneRecurs(octree, octree.getRoot(), 0, depth, num_pruned); + if (num_pruned == 0) + break; + } +} + bool Octree::operator==(const Octree& rhs) const { using namespace tesseract_common; diff --git a/tesseract_geometry/src/geometries/plane.cpp b/tesseract_geometry/src/geometries/plane.cpp index 5a541538d99..38e444a329f 100644 --- a/tesseract_geometry/src/geometries/plane.cpp +++ b/tesseract_geometry/src/geometries/plane.cpp @@ -34,6 +34,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Plane::Plane(double a, double b, double c, double d) : Geometry(GeometryType::PLANE), a_(a), b_(b), c_(c), d_(d) {} + +double Plane::getA() const { return a_; } +double Plane::getB() const { return b_; } +double Plane::getC() const { return c_; } +double Plane::getD() const { return d_; } + +Geometry::Ptr Plane::clone() const { return std::make_shared(a_, b_, c_, d_); } + bool Plane::operator==(const Plane& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/polygon_mesh.cpp b/tesseract_geometry/src/geometries/polygon_mesh.cpp index a69d5df595c..83674c7c4c0 100644 --- a/tesseract_geometry/src/geometries/polygon_mesh.cpp +++ b/tesseract_geometry/src/geometries/polygon_mesh.cpp @@ -35,10 +35,92 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include +#include namespace tesseract_geometry { +PolygonMesh::PolygonMesh(std::shared_ptr vertices, + std::shared_ptr faces, + std::shared_ptr resource, + const Eigen::Vector3d& scale, // NOLINT + std::shared_ptr normals, + std::shared_ptr vertex_colors, + std::shared_ptr mesh_material, + std::shared_ptr>> mesh_textures, + GeometryType type) + : Geometry(type) + , vertices_(std::move(vertices)) + , faces_(std::move(faces)) + , vertex_count_(static_cast(vertices_->size())) + , resource_(std::move(resource)) + , scale_(scale) + , normals_(std::move(normals)) + , vertex_colors_(std::move(vertex_colors)) + , mesh_material_(std::move(mesh_material)) + , mesh_textures_(std::move(mesh_textures)) +{ + for (int i = 0; i < faces_->size(); ++i) + { + ++face_count_; + int num_verts = (*faces_)(i); // NOLINT + i += num_verts; + } +} + +PolygonMesh::PolygonMesh(std::shared_ptr vertices, + std::shared_ptr faces, + int face_count, + tesseract_common::Resource::ConstPtr resource, + const Eigen::Vector3d& scale, // NOLINT + std::shared_ptr normals, + std::shared_ptr vertex_colors, + MeshMaterial::Ptr mesh_material, + std::shared_ptr> mesh_textures, + GeometryType type) + : Geometry(type) + , vertices_(std::move(vertices)) + , faces_(std::move(faces)) + , vertex_count_(static_cast(vertices_->size())) + , face_count_(face_count) + , resource_(std::move(resource)) + , scale_(scale) + , normals_(std::move(normals)) + , vertex_colors_(std::move(vertex_colors)) + , mesh_material_(std::move(mesh_material)) + , mesh_textures_(std::move(mesh_textures)) +{ +} + +const std::shared_ptr& PolygonMesh::getVertices() const { return vertices_; } + +const std::shared_ptr& PolygonMesh::getFaces() const { return faces_; } + +int PolygonMesh::getVertexCount() const { return vertex_count_; } + +int PolygonMesh::getFaceCount() const { return face_count_; } + +tesseract_common::Resource::ConstPtr PolygonMesh::getResource() const { return resource_; } + +const Eigen::Vector3d& PolygonMesh::getScale() const { return scale_; } + +const std::shared_ptr& PolygonMesh::getNormals() const { return normals_; } + +const std::shared_ptr& PolygonMesh::getVertexColors() const +{ + return vertex_colors_; +} + +MeshMaterial::ConstPtr PolygonMesh::getMaterial() const { return mesh_material_; } + +const std::shared_ptr>& PolygonMesh::getTextures() const { return mesh_textures_; } + +Geometry::Ptr PolygonMesh::clone() const +{ + return std::make_shared(vertices_, faces_, face_count_, resource_, scale_); +} + bool PolygonMesh::operator==(const PolygonMesh& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/sdf_mesh.cpp b/tesseract_geometry/src/geometries/sdf_mesh.cpp index 45cf87accfe..e793b3a7e58 100644 --- a/tesseract_geometry/src/geometries/sdf_mesh.cpp +++ b/tesseract_geometry/src/geometries/sdf_mesh.cpp @@ -29,11 +29,63 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tesseract_geometry { +SDFMesh::SDFMesh(std::shared_ptr vertices, + std::shared_ptr triangles, + std::shared_ptr resource, + const Eigen::Vector3d& scale, + std::shared_ptr normals, + std::shared_ptr vertex_colors, + std::shared_ptr mesh_material, + std::shared_ptr>> mesh_textures) + : PolygonMesh(std::move(vertices), + std::move(triangles), + std::move(resource), + scale, + std::move(normals), + std::move(vertex_colors), + std::move(mesh_material), + std::move(mesh_textures), + GeometryType::SDF_MESH) +{ + if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE +} + +SDFMesh::SDFMesh(std::shared_ptr vertices, + std::shared_ptr triangles, + int triangle_count, + std::shared_ptr resource, + const Eigen::Vector3d& scale, + std::shared_ptr normals, + std::shared_ptr vertex_colors, + std::shared_ptr mesh_material, + std::shared_ptr>> mesh_textures) + : PolygonMesh(std::move(vertices), + std::move(triangles), + triangle_count, + std::move(resource), + scale, + std::move(normals), + std::move(vertex_colors), + std::move(mesh_material), + std::move(mesh_textures), + GeometryType::SDF_MESH) +{ + if ((static_cast(getFaceCount()) * 4) != getFaces()->size()) + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE +} + +Geometry::Ptr SDFMesh::clone() const +{ + return std::make_shared(getVertices(), getFaces(), getFaceCount(), getResource(), getScale()); +} + bool SDFMesh::operator==(const SDFMesh& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/geometries/sphere.cpp b/tesseract_geometry/src/geometries/sphere.cpp index ab9c85570e1..88fb6fc494d 100644 --- a/tesseract_geometry/src/geometries/sphere.cpp +++ b/tesseract_geometry/src/geometries/sphere.cpp @@ -34,6 +34,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Sphere::Sphere(double r) : Geometry(GeometryType::SPHERE), r_(r) {} + +double Sphere::getRadius() const { return r_; } + +Geometry::Ptr Sphere::clone() const { return std::make_shared(r_); } + bool Sphere::operator==(const Sphere& rhs) const { bool equal = true; diff --git a/tesseract_geometry/src/utils.cpp b/tesseract_geometry/src/utils.cpp index 593031b2b9c..fd6087a0298 100644 --- a/tesseract_geometry/src/utils.cpp +++ b/tesseract_geometry/src/utils.cpp @@ -27,6 +27,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include diff --git a/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp b/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp index 3fc1560e6fc..25aa3334984 100644 --- a/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_serialization_unit.cpp @@ -26,6 +26,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -108,13 +109,13 @@ TEST(TesseractGeometrySerializeUnit, Octree) // NOLINT pc.points.emplace_back(-.5, 0.5, 0.5); { auto object = - std::make_shared(pc, 1, tesseract_geometry::Octree::SubType::BOX, false, true); + std::make_shared(pc, 1, tesseract_geometry::OctreeSubType::BOX, false, true); tesseract_common::testSerialization(*object, "Binary_Octree"); tesseract_common::testSerializationDerivedClass(object, "Binary_Octree"); } { auto object = - std::make_shared(pc, 1, tesseract_geometry::Octree::SubType::BOX, false, false); + std::make_shared(pc, 1, tesseract_geometry::OctreeSubType::BOX, false, false); tesseract_common::testSerialization(*object, "Full_Octree"); tesseract_common::testSerializationDerivedClass(object, "Full_Octree"); } diff --git a/tesseract_geometry/test/tesseract_geometry_unit.cpp b/tesseract_geometry/test/tesseract_geometry_unit.cpp index e669a3d86f8..bc746161714 100644 --- a/tesseract_geometry/test/tesseract_geometry_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_unit.cpp @@ -25,7 +25,7 @@ TEST(TesseractGeometryUnit, Instantiation) // NOLINT auto convex_mesh = std::make_shared(vertices, faces); auto mesh = std::make_shared(vertices, faces); auto sdf_mesh = std::make_shared(vertices, faces); - auto octree = std::make_shared(nullptr, tesseract_geometry::Octree::SubType::BOX); + auto octree = std::make_shared(nullptr, tesseract_geometry::OctreeSubType::BOX); // Instead making this depend on pcl it expects the structure to have a member called points which is a vector // of another object with has float members x, y and z. @@ -43,7 +43,7 @@ TEST(TesseractGeometryUnit, Instantiation) // NOLINT TestPointCloud pc; auto octree_pc = - std::make_shared(pc, 0.01, tesseract_geometry::Octree::SubType::BOX, false); + std::make_shared(pc, 0.01, tesseract_geometry::OctreeSubType::BOX, false); } TEST(TesseractGeometryUnit, Box) // NOLINT @@ -380,14 +380,14 @@ TEST(TesseractGeometryUnit, Octree) // NOLINT }; TestPointCloud pc; - auto geom = std::make_shared(pc, 0.01, tesseract_geometry::Octree::SubType::BOX, false); + auto geom = std::make_shared(pc, 0.01, tesseract_geometry::OctreeSubType::BOX, false); EXPECT_TRUE(geom->getOctree() != nullptr); - EXPECT_TRUE(geom->getSubType() == tesseract_geometry::Octree::SubType::BOX); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_EQ(geom->getType(), tesseract_geometry::GeometryType::OCTREE); auto geom_clone = geom->clone(); EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getOctree() != nullptr); - EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getSubType() == tesseract_geometry::Octree::SubType::BOX); + EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_EQ(geom_clone->getType(), tesseract_geometry::GeometryType::OCTREE); // Test isIdentical diff --git a/tesseract_kinematics/core/CMakeLists.txt b/tesseract_kinematics/core/CMakeLists.txt index 8c36907862a..a30ec423793 100644 --- a/tesseract_kinematics/core/CMakeLists.txt +++ b/tesseract_kinematics/core/CMakeLists.txt @@ -5,6 +5,7 @@ add_library( src/joint_group.cpp src/kinematic_group.cpp src/kinematics_plugin_factory.cpp + src/utils.cpp src/validate.cpp) target_link_libraries( ${PROJECT_NAME}_core diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/forward_kinematics.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/forward_kinematics.h index 5e5643e6c91..49eb35ab014 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/forward_kinematics.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/forward_kinematics.h @@ -31,12 +31,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/fwd.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/fwd.h new file mode 100644 index 00000000000..78395a9cf74 --- /dev/null +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/fwd.h @@ -0,0 +1,48 @@ +/** + * @file fwd.h + * @brief Tesseract Kinematics Forward Declarations. + * + * @author Levi Armstrong + * @date February 21, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_KINEMATICS_FWD_H +#define TESSERACT_KINEMATICS_FWD_H + +namespace tesseract_kinematics +{ +class ForwardKinematics; +class InverseKinematics; +class JointGroup; +struct KinGroupIKInput; +class KinematicGroup; +class InvKinFactory; +class FwdKinFactory; +class KinematicsPluginFactory; +class REPInvKinFactory; +class REPInvKin; +class ROPInvKinFactory; +class ROPInvKin; +struct URParameters; +struct ManipulabilityEllipsoid; +struct Manipulability; +} // namespace tesseract_kinematics + +#endif // TESSERACT_KINEMATICS_FWD_H diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/inverse_kinematics.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/inverse_kinematics.h index c139c78f124..22bb2994244 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/inverse_kinematics.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/inverse_kinematics.h @@ -31,11 +31,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include #include namespace tesseract_kinematics diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h index 89206b803ba..90cec8b6a78 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h @@ -26,9 +26,11 @@ #ifndef TESSERACT_KINEMATICS_JOINT_GROUP_H #define TESSERACT_KINEMATICS_JOINT_GROUP_H -#include +#include +#include +#include +#include #include -#include namespace tesseract_kinematics { @@ -202,7 +204,7 @@ class JointGroup protected: std::string name_; tesseract_scene_graph::SceneState state_; - tesseract_scene_graph::StateSolver::UPtr state_solver_; + std::unique_ptr state_solver_; std::vector joint_names_; std::vector link_names_; std::vector static_link_names_; diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h index 2dca00d701c..6e7a6727332 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h @@ -33,10 +33,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include namespace tesseract_kinematics { +class InverseKinematics; /** * @brief Structure containing the data required to solve inverse kinematics * @details This structure provides the ability to specify IK targets for arbitrary tool links and defined with respect @@ -50,10 +51,7 @@ struct KinGroupIKInput EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP - KinGroupIKInput(const Eigen::Isometry3d& p, std::string wf, std::string tl) - : pose(p), working_frame(std::move(wf)), tip_link_name(std::move(tl)) - { - } + KinGroupIKInput(const Eigen::Isometry3d& p, std::string wf, std::string tl); KinGroupIKInput() = default; @@ -101,7 +99,7 @@ class KinematicGroup : public JointGroup */ KinematicGroup(std::string name, std::vector joint_names, - InverseKinematics::UPtr inv_kin, + std::unique_ptr inv_kin, const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state); @@ -149,7 +147,7 @@ class KinematicGroup : public JointGroup std::vector joint_names_; bool reorder_required_{ false }; std::vector inv_kin_joint_map_; - InverseKinematics::UPtr inv_kin_; + std::unique_ptr inv_kin_; Eigen::Isometry3d inv_to_fwd_base_{ Eigen::Isometry3d::Identity() }; std::vector working_frames_; std::unordered_map inv_tip_links_map_; diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h index 2844a214934..3117fec6edb 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/kinematics_plugin_factory.h @@ -34,12 +34,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include -#include +#include +#include #include -#include +#include +#include // clang-format off #define TESSERACT_ADD_FWD_KIN_PLUGIN(DERIVED_CLASS, ALIAS) \ @@ -53,6 +52,8 @@ namespace tesseract_kinematics { /** @brief Forward declare Plugin Factory */ class KinematicsPluginFactory; +class InverseKinematics; +class ForwardKinematics; /** @brief Define a inverse kinematics plugin which the factory can create an instance */ class InvKinFactory @@ -71,11 +72,11 @@ class InvKinFactory * @param plugin_factory Provide access to the plugin factory so plugins and load plugins * @return If failed to create, nullptr is returned. */ - virtual InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const = 0; + virtual std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const = 0; protected: static const std::string SECTION_NAME; @@ -99,11 +100,11 @@ class FwdKinFactory * @param plugin_factory Provide access to the plugin factory so plugins and load plugins * @return If failed to create, nullptr is returned. */ - virtual ForwardKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const = 0; + virtual std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const = 0; protected: static const std::string SECTION_NAME; @@ -246,10 +247,10 @@ class KinematicsPluginFactory * @param scene_graph The scene graph * @param scene_state The scene state */ - ForwardKinematics::UPtr createFwdKin(const std::string& group_name, - const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state) const; + std::unique_ptr createFwdKin(const std::string& group_name, + const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state) const; /** * @brief Get inverse kinematics object given group name and solver name @@ -259,10 +260,10 @@ class KinematicsPluginFactory * @param scene_graph The scene graph * @param scene_state The scene state */ - InverseKinematics::UPtr createInvKin(const std::string& group_name, - const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state) const; + std::unique_ptr createInvKin(const std::string& group_name, + const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state) const; /** * @brief Get forward kinematics object given plugin info @@ -270,10 +271,10 @@ class KinematicsPluginFactory * @param scene_graph The scene graph * @param scene_state The scene state */ - ForwardKinematics::UPtr createFwdKin(const std::string& solver_name, - const tesseract_common::PluginInfo& plugin_info, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state) const; + std::unique_ptr createFwdKin(const std::string& solver_name, + const tesseract_common::PluginInfo& plugin_info, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state) const; /** * @brief Get inverse kinematics object given plugin info @@ -281,10 +282,10 @@ class KinematicsPluginFactory * @param scene_graph The scene graph * @param scene_state The scene state */ - InverseKinematics::UPtr createInvKin(const std::string& solver_name, - const tesseract_common::PluginInfo& plugin_info, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state) const; + std::unique_ptr createInvKin(const std::string& solver_name, + const tesseract_common::PluginInfo& plugin_info, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state) const; /** * @brief Save the plugin information to a yaml config file diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_factory.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_factory.h index c07c606be92..7d620fe1e4d 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_factory.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_factory.h @@ -32,11 +32,11 @@ namespace tesseract_kinematics { class REPInvKinFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(REPInvKinFactoriesAnchor) diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_inv_kin.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_inv_kin.h index 04422548089..14efaa57579 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_inv_kin.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/rep_inv_kin.h @@ -25,21 +25,20 @@ */ #ifndef TESSERACT_KINEMATICS_REP_INVERSE_KINEMATICS_H #define TESSERACT_KINEMATICS_REP_INVERSE_KINEMATICS_H + #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include - -#include -#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include -#include -#include namespace tesseract_kinematics { static const std::string DEFAULT_REP_INV_KIN_SOLVER_NAME = "REPInvKin"; +class ForwardKinematics; /** * @brief Robot With External Positioner Inverse kinematic implementation. @@ -79,7 +78,7 @@ class REPInvKin : public InverseKinematics const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name = DEFAULT_REP_INV_KIN_SOLVER_NAME); @@ -98,7 +97,7 @@ class REPInvKin : public InverseKinematics const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& positioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name = DEFAULT_REP_INV_KIN_SOLVER_NAME); @@ -117,7 +116,7 @@ class REPInvKin : public InverseKinematics private: std::vector joint_names_; InverseKinematics::UPtr manip_inv_kin_; - ForwardKinematics::UPtr positioner_fwd_kin_; + std::unique_ptr positioner_fwd_kin_; std::string working_frame_; std::string manip_tip_link_; double manip_reach_{ 0 }; @@ -130,7 +129,7 @@ class REPInvKin : public InverseKinematics const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& poitioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name = DEFAULT_REP_INV_KIN_SOLVER_NAME); diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_factory.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_factory.h index 76128d48500..88fc8c39374 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_factory.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_factory.h @@ -32,11 +32,11 @@ namespace tesseract_kinematics { class ROPInvKinFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(ROPInvKinFactoriesAnchor) diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_inv_kin.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_inv_kin.h index ea6517046f3..e488ce182b2 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_inv_kin.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/rop_inv_kin.h @@ -28,19 +28,17 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include - -#include -#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include -#include -#include namespace tesseract_kinematics { static const std::string DEFAULT_ROP_INV_KIN_SOLVER_NAME = "ROPInvKin"; +class ForwardKinematics; /** * @brief Robot on Positioner Inverse kinematic implementation. @@ -79,7 +77,7 @@ class ROPInvKin : public InverseKinematics const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name = DEFAULT_ROP_INV_KIN_SOLVER_NAME); @@ -100,7 +98,7 @@ class ROPInvKin : public InverseKinematics const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& positioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name = DEFAULT_ROP_INV_KIN_SOLVER_NAME); @@ -119,7 +117,7 @@ class ROPInvKin : public InverseKinematics private: std::vector joint_names_; InverseKinematics::UPtr manip_inv_kin_; - ForwardKinematics::UPtr positioner_fwd_kin_; + std::unique_ptr positioner_fwd_kin_; std::string manip_tip_link_; std::string positioner_tip_link_; double manip_reach_{ 0 }; @@ -132,7 +130,7 @@ class ROPInvKin : public InverseKinematics const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& poitioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name = DEFAULT_ROP_INV_KIN_SOLVER_NAME); diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/types.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/types.h index c18b7990321..10f865e0866 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/types.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/types.h @@ -29,11 +29,9 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include - namespace tesseract_kinematics { /** @brief The inverse kinematics solutions container */ diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h index 23f89db8114..b8be3ec2155 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h @@ -30,19 +30,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace tesseract_kinematics { template using VectorX = Eigen::Matrix; +class JointGroup; +class ForwardKinematics; + /** * @brief Numerically calculate a jacobian. This is mainly used for testing * @param jacobian (Return) The jacobian which gets filled out. @@ -51,39 +52,12 @@ using VectorX = Eigen::Matrix; * @param link_name The link_name for which the jacobian should be calculated * @param link_point The point on the link for which to calculate the jacobian */ -inline static void numericalJacobian(Eigen::Ref jacobian, - const Eigen::Isometry3d& change_base, - const tesseract_kinematics::ForwardKinematics& kin, - const Eigen::Ref& joint_values, - const std::string& link_name, - const Eigen::Ref& link_point) -{ - Eigen::VectorXd njvals; - double delta = 1e-8; - tesseract_common::TransformMap poses = kin.calcFwdKin(joint_values); - Eigen::Isometry3d pose{ change_base * poses[link_name] }; - - for (int i = 0; i < static_cast(joint_values.size()); ++i) - { - njvals = joint_values; - njvals[i] += delta; - Eigen::Isometry3d updated_pose = kin.calcFwdKin(njvals)[link_name]; - updated_pose = change_base * updated_pose; - - Eigen::Vector3d temp{ pose * link_point }; - Eigen::Vector3d temp2{ updated_pose * link_point }; - jacobian(0, i) = (temp2.x() - temp.x()) / delta; - jacobian(1, i) = (temp2.y() - temp.y()) / delta; - jacobian(2, i) = (temp2.z() - temp.z()) / delta; - - Eigen::Vector3d omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() * - updated_pose.rotation())) / - delta; - jacobian(3, i) = omega(0); - jacobian(4, i) = omega(1); - jacobian(5, i) = omega(2); - } -} +void numericalJacobian(Eigen::Ref jacobian, + const Eigen::Isometry3d& change_base, + const tesseract_kinematics::ForwardKinematics& kin, + const Eigen::Ref& joint_values, + const std::string& link_name, + const Eigen::Ref& link_point); /** * @brief Numerically calculate a jacobian. This is mainly used for testing @@ -93,39 +67,12 @@ inline static void numericalJacobian(Eigen::Ref jacobian, * @param link_name The link_name for which the jacobian should be calculated * @param link_point The point on the link for which to calculate the jacobian */ -inline static void numericalJacobian(Eigen::Ref jacobian, - const Eigen::Isometry3d& change_base, - const JointGroup& joint_group, - const Eigen::Ref& joint_values, - const std::string& link_name, - const Eigen::Ref& link_point) -{ - Eigen::VectorXd njvals; - double delta = 1e-8; - tesseract_common::TransformMap poses = joint_group.calcFwdKin(joint_values); - Eigen::Isometry3d pose = change_base * poses[link_name]; - - for (int i = 0; i < static_cast(joint_values.size()); ++i) - { - njvals = joint_values; - njvals[i] += delta; - tesseract_common::TransformMap updated_poses = joint_group.calcFwdKin(njvals); - Eigen::Isometry3d updated_pose = change_base * updated_poses[link_name]; - - Eigen::Vector3d temp = pose * link_point; - Eigen::Vector3d temp2 = updated_pose * link_point; - jacobian(0, i) = (temp2.x() - temp.x()) / delta; - jacobian(1, i) = (temp2.y() - temp.y()) / delta; - jacobian(2, i) = (temp2.z() - temp.z()) / delta; - - Eigen::VectorXd omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() * - updated_pose.rotation())) / - delta; - jacobian(3, i) = omega(0); - jacobian(4, i) = omega(1); - jacobian(5, i) = omega(2); - } -} +void numericalJacobian(Eigen::Ref jacobian, + const Eigen::Isometry3d& change_base, + const JointGroup& joint_group, + const Eigen::Ref& joint_values, + const std::string& link_name, + const Eigen::Ref& link_point); /** * @brief Solve equation Ax=b for x @@ -135,47 +82,9 @@ inline static void numericalJacobian(Eigen::Ref jacobian, * @param x Output vector (represents joint values) * @return True if solver completes properly */ -inline static bool solvePInv(const Eigen::Ref& A, - const Eigen::Ref& b, - Eigen::Ref x) -{ - const double eps = 0.00001; // TODO: Turn into class member var - const double lambda = 0.01; // TODO: Turn into class member var - - if ((A.rows() == 0) || (A.cols() == 0)) - { - CONSOLE_BRIDGE_logError("Empty matrices not supported in solvePinv()"); - return false; - } - - if (A.rows() != b.size()) - { - CONSOLE_BRIDGE_logError("Matrix size mismatch: A(%d, %d), b(%d)", A.rows(), A.cols(), b.size()); - return false; - } - - // Calculate A+ (pseudoinverse of A) = V S+ U*, where U* is Hermition of U (just transpose if all values of U are - // real) - // in order to solve Ax=b -> x*=A+ b - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); - const Eigen::MatrixXd& U = svd.matrixU(); - const Eigen::VectorXd& Sv = svd.singularValues(); - const Eigen::MatrixXd& V = svd.matrixV(); - - // calculate the reciprocal of Singular-Values - // damp inverse with lambda so that inverse doesn't oscillate near solution - long int nSv = Sv.size(); - Eigen::VectorXd inv_Sv(nSv); - for (long int i = 0; i < nSv; ++i) - { - if (fabs(Sv(i)) > eps) - inv_Sv(i) = 1 / Sv(i); - else - inv_Sv(i) = Sv(i) / (Sv(i) * Sv(i) + lambda * lambda); - } - x = V * inv_Sv.asDiagonal() * U.transpose() * b; - return true; -} +bool solvePInv(const Eigen::Ref& A, + const Eigen::Ref& b, + Eigen::Ref x); /** * @brief Calculate Damped Pseudoinverse @@ -186,41 +95,10 @@ inline static bool solvePInv(const Eigen::Ref& A, * @param lambda Damping factor * @return True if Pseudoinverse completes properly */ -inline static bool dampedPInv(const Eigen::Ref& A, - Eigen::Ref P, - const double eps = 0.011, - const double lambda = 0.01) -{ - if ((A.rows() == 0) || (A.cols() == 0)) - { - CONSOLE_BRIDGE_logError("Empty matrices not supported in dampedPInv()"); - return false; - } - - // Calculate A+ (pseudoinverse of A) = V S+ U*, where U* is Hermition of U (just transpose if all values of U are - // real) - // in order to solve Ax=b -> x*=A+ b - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); - const Eigen::MatrixXd& U = svd.matrixU(); - const Eigen::VectorXd& Sv = svd.singularValues(); - const Eigen::MatrixXd& V = svd.matrixV(); - - // calculate the reciprocal of Singular-Values - // damp inverse with lambda so that inverse doesn't oscillate near solution - long int nSv = Sv.size(); - Eigen::VectorXd inv_Sv(nSv); - for (long int i = 0; i < nSv; ++i) - { - if (fabs(Sv(i)) > eps) - inv_Sv(i) = 1 / Sv(i); - else - { - inv_Sv(i) = Sv(i) / (Sv(i) * Sv(i) + lambda * lambda); - } - } - P = V * inv_Sv.asDiagonal() * U.transpose(); - return true; -} +bool dampedPInv(const Eigen::Ref& A, + Eigen::Ref P, + const double eps = 0.011, + const double lambda = 0.01); /** * @brief Check if the provided jacobian is near a singularity @@ -230,12 +108,7 @@ inline static bool dampedPInv(const Eigen::Ref& A, * @param threshold The threshold that all singular values must be greater than or equal to not be considered near a * singularity */ -inline bool isNearSingularity(const Eigen::Ref& jacobian, double threshold = 0.01) -{ - Eigen::JacobiSVD svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - const Eigen::VectorXd& sv = svd.singularValues(); - return (sv.tail(1).value() < threshold); -} +bool isNearSingularity(const Eigen::Ref& jacobian, double threshold = 0.01); /** @brief Used to store Manipulability and Force Ellipsoid data */ struct ManipulabilityEllipsoid @@ -298,57 +171,7 @@ struct Manipulability * @param jacobian The jacobian used to calculate manipulability * @return The manipulability data */ -inline Manipulability calcManipulability(const Eigen::Ref& jacobian) -{ - Manipulability manip; - Eigen::MatrixXd jacob_linear = jacobian.topRows(3); - Eigen::MatrixXd jacob_angular = jacobian.bottomRows(3); - - auto fn = [](const Eigen::MatrixXd& m) { - ManipulabilityEllipsoid data; - Eigen::SelfAdjointEigenSolver sm(m, Eigen::DecompositionOptions::EigenvaluesOnly); - data.eigen_values = sm.eigenvalues().real(); - - // Set eigenvalues near zero to zero. This also implies zero volume - for (Eigen::Index i = 0; i < data.eigen_values.size(); ++i) - { - if (tesseract_common::almostEqualRelativeAndAbs(data.eigen_values[i], 0)) - data.eigen_values[i] = +0; - } - - // If the minimum eigen value is approximately zero set measure and condition to max double - if (tesseract_common::almostEqualRelativeAndAbs(data.eigen_values.minCoeff(), 0)) - { - data.measure = std::numeric_limits::max(); - data.condition = std::numeric_limits::max(); - } - else - { - data.condition = data.eigen_values.maxCoeff() / data.eigen_values.minCoeff(); - data.measure = std::sqrt(data.condition); - } - - data.volume = std::sqrt(data.eigen_values.prod()); - - return data; - }; - - Eigen::MatrixXd a = jacobian * jacobian.transpose(); - Eigen::MatrixXd a_linear = jacob_linear * jacob_linear.transpose(); - Eigen::MatrixXd a_angular = jacob_angular * jacob_angular.transpose(); - manip.m = fn(a); - manip.m_linear = fn(a_linear); - manip.m_angular = fn(a_angular); - - Eigen::MatrixXd a_inv = a.inverse(); - Eigen::MatrixXd a_linear_inv = a_linear.inverse(); - Eigen::MatrixXd a_angular_inv = a_angular.inverse(); - manip.f = fn(a_inv); - manip.f_linear = fn(a_linear_inv); - manip.f_angular = fn(a_angular_inv); - - return manip; -} +Manipulability calcManipulability(const Eigen::Ref& jacobian); /** * @brief This a recursive function for calculating all permutations of the redundant solutions. diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/validate.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/validate.h index d6757f4ac75..d771ff70b82 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/validate.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/validate.h @@ -26,10 +26,9 @@ #ifndef TESSERACT_KINEMATICS_VALIDATE_H #define TESSERACT_KINEMATICS_VALIDATE_H -#include - namespace tesseract_kinematics { +class KinematicGroup; /** * @brief This compares calcFwdKin to calcInvKin for a KinematicGroup. * diff --git a/tesseract_kinematics/core/src/joint_group.cpp b/tesseract_kinematics/core/src/joint_group.cpp index a4e1ba49006..0fdff254677 100644 --- a/tesseract_kinematics/core/src/joint_group.cpp +++ b/tesseract_kinematics/core/src/joint_group.cpp @@ -31,8 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include +#include #include +#include + namespace tesseract_kinematics { JointGroup::JointGroup(std::string name, diff --git a/tesseract_kinematics/core/src/kinematic_group.cpp b/tesseract_kinematics/core/src/kinematic_group.cpp index 175060c1a66..059b2d4dbb1 100644 --- a/tesseract_kinematics/core/src/kinematic_group.cpp +++ b/tesseract_kinematics/core/src/kinematic_group.cpp @@ -29,16 +29,23 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include -#include +#include +#include namespace tesseract_kinematics { +KinGroupIKInput::KinGroupIKInput(const Eigen::Isometry3d& p, std::string wf, std::string tl) + : pose(p), working_frame(std::move(wf)), tip_link_name(std::move(tl)) +{ +} + KinematicGroup::KinematicGroup(std::string name, std::vector joint_names, - InverseKinematics::UPtr inv_kin, + std::unique_ptr inv_kin, const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state) : JointGroup(std::move(name), joint_names, scene_graph, scene_state) diff --git a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp index b97342913f7..6d1b61e7e44 100644 --- a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp +++ b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp @@ -28,6 +28,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include +#include #include #include #include @@ -209,7 +213,7 @@ std::string KinematicsPluginFactory::getDefaultInvKinPlugin(const std::string& g return group_it->second.default_plugin; } -ForwardKinematics::UPtr +std::unique_ptr KinematicsPluginFactory::createFwdKin(const std::string& group_name, const std::string& solver_name, const tesseract_scene_graph::SceneGraph& scene_graph, @@ -238,7 +242,7 @@ KinematicsPluginFactory::createFwdKin(const std::string& group_name, return createFwdKin(solver_name, solver_it->second, scene_graph, scene_state); } -ForwardKinematics::UPtr +std::unique_ptr KinematicsPluginFactory::createFwdKin(const std::string& solver_name, const tesseract_common::PluginInfo& plugin_info, const tesseract_scene_graph::SceneGraph& scene_graph, @@ -266,7 +270,7 @@ KinematicsPluginFactory::createFwdKin(const std::string& solver_name, } } -InverseKinematics::UPtr +std::unique_ptr KinematicsPluginFactory::createInvKin(const std::string& group_name, const std::string& solver_name, const tesseract_scene_graph::SceneGraph& scene_graph, @@ -295,7 +299,7 @@ KinematicsPluginFactory::createInvKin(const std::string& group_name, return createInvKin(solver_name, solver_it->second, scene_graph, scene_state); } -InverseKinematics::UPtr +std::unique_ptr KinematicsPluginFactory::createInvKin(const std::string& solver_name, const tesseract_common::PluginInfo& plugin_info, const tesseract_scene_graph::SceneGraph& scene_graph, diff --git a/tesseract_kinematics/core/src/rep_factory.cpp b/tesseract_kinematics/core/src/rep_factory.cpp index ff1ed85bb3e..52710417b9a 100644 --- a/tesseract_kinematics/core/src/rep_factory.cpp +++ b/tesseract_kinematics/core/src/rep_factory.cpp @@ -25,14 +25,17 @@ */ #include #include +#include +#include +#include namespace tesseract_kinematics { -InverseKinematics::UPtr REPInvKinFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const +std::unique_ptr REPInvKinFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const { ForwardKinematics::UPtr fwd_kin; InverseKinematics::UPtr inv_kin; diff --git a/tesseract_kinematics/core/src/rep_inv_kin.cpp b/tesseract_kinematics/core/src/rep_inv_kin.cpp index 9741b22dc2d..9b766374afd 100644 --- a/tesseract_kinematics/core/src/rep_inv_kin.cpp +++ b/tesseract_kinematics/core/src/rep_inv_kin.cpp @@ -23,12 +23,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include +#include +#include namespace tesseract_kinematics { @@ -39,7 +39,7 @@ REPInvKin::REPInvKin(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name) { @@ -74,7 +74,7 @@ REPInvKin::REPInvKin(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& positioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name) @@ -93,7 +93,7 @@ void REPInvKin::init(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& poitioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name) diff --git a/tesseract_kinematics/core/src/rop_factory.cpp b/tesseract_kinematics/core/src/rop_factory.cpp index fa91616096f..7edf18353e0 100644 --- a/tesseract_kinematics/core/src/rop_factory.cpp +++ b/tesseract_kinematics/core/src/rop_factory.cpp @@ -26,14 +26,17 @@ #include #include +#include +#include +#include namespace tesseract_kinematics { -InverseKinematics::UPtr ROPInvKinFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const +std::unique_ptr ROPInvKinFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const { ForwardKinematics::UPtr fwd_kin; InverseKinematics::UPtr inv_kin; diff --git a/tesseract_kinematics/core/src/rop_inv_kin.cpp b/tesseract_kinematics/core/src/rop_inv_kin.cpp index e5fd859a0d5..e82cd130b95 100644 --- a/tesseract_kinematics/core/src/rop_inv_kin.cpp +++ b/tesseract_kinematics/core/src/rop_inv_kin.cpp @@ -23,12 +23,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include +#include +#include namespace tesseract_kinematics { @@ -39,7 +39,7 @@ ROPInvKin::ROPInvKin(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name) { @@ -74,7 +74,7 @@ ROPInvKin::ROPInvKin(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& positioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name) @@ -93,7 +93,7 @@ void ROPInvKin::init(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_scene_graph::SceneState& scene_state, InverseKinematics::UPtr manipulator, double manipulator_reach, - ForwardKinematics::UPtr positioner, + std::unique_ptr positioner, const Eigen::MatrixX2d& poitioner_sample_range, const Eigen::VectorXd& positioner_sample_resolution, std::string solver_name) diff --git a/tesseract_kinematics/core/src/utils.cpp b/tesseract_kinematics/core/src/utils.cpp new file mode 100644 index 00000000000..4bea5257fbd --- /dev/null +++ b/tesseract_kinematics/core/src/utils.cpp @@ -0,0 +1,242 @@ +/** + * @file utils.cpp + * @brief Kinematics utility functions. + * + * @author Levi Armstrong + * @date April 15, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2013, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_kinematics +{ +void numericalJacobian(Eigen::Ref jacobian, + const Eigen::Isometry3d& change_base, + const tesseract_kinematics::ForwardKinematics& kin, + const Eigen::Ref& joint_values, + const std::string& link_name, + const Eigen::Ref& link_point) +{ + Eigen::VectorXd njvals; + double delta = 1e-8; + tesseract_common::TransformMap poses = kin.calcFwdKin(joint_values); + Eigen::Isometry3d pose{ change_base * poses[link_name] }; + + for (int i = 0; i < static_cast(joint_values.size()); ++i) + { + njvals = joint_values; + njvals[i] += delta; + Eigen::Isometry3d updated_pose = kin.calcFwdKin(njvals)[link_name]; + updated_pose = change_base * updated_pose; + + Eigen::Vector3d temp{ pose * link_point }; + Eigen::Vector3d temp2{ updated_pose * link_point }; + jacobian(0, i) = (temp2.x() - temp.x()) / delta; + jacobian(1, i) = (temp2.y() - temp.y()) / delta; + jacobian(2, i) = (temp2.z() - temp.z()) / delta; + + Eigen::Vector3d omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() * + updated_pose.rotation())) / + delta; + jacobian(3, i) = omega(0); + jacobian(4, i) = omega(1); + jacobian(5, i) = omega(2); + } +} + +void numericalJacobian(Eigen::Ref jacobian, + const Eigen::Isometry3d& change_base, + const JointGroup& joint_group, + const Eigen::Ref& joint_values, + const std::string& link_name, + const Eigen::Ref& link_point) +{ + Eigen::VectorXd njvals; + double delta = 1e-8; + tesseract_common::TransformMap poses = joint_group.calcFwdKin(joint_values); + Eigen::Isometry3d pose = change_base * poses[link_name]; + + for (int i = 0; i < static_cast(joint_values.size()); ++i) + { + njvals = joint_values; + njvals[i] += delta; + tesseract_common::TransformMap updated_poses = joint_group.calcFwdKin(njvals); + Eigen::Isometry3d updated_pose = change_base * updated_poses[link_name]; + + Eigen::Vector3d temp = pose * link_point; + Eigen::Vector3d temp2 = updated_pose * link_point; + jacobian(0, i) = (temp2.x() - temp.x()) / delta; + jacobian(1, i) = (temp2.y() - temp.y()) / delta; + jacobian(2, i) = (temp2.z() - temp.z()) / delta; + + Eigen::VectorXd omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() * + updated_pose.rotation())) / + delta; + jacobian(3, i) = omega(0); + jacobian(4, i) = omega(1); + jacobian(5, i) = omega(2); + } +} + +bool solvePInv(const Eigen::Ref& A, + const Eigen::Ref& b, + Eigen::Ref x) +{ + const double eps = 0.00001; // TODO: Turn into class member var + const double lambda = 0.01; // TODO: Turn into class member var + + if ((A.rows() == 0) || (A.cols() == 0)) + { + CONSOLE_BRIDGE_logError("Empty matrices not supported in solvePinv()"); + return false; + } + + if (A.rows() != b.size()) + { + CONSOLE_BRIDGE_logError("Matrix size mismatch: A(%d, %d), b(%d)", A.rows(), A.cols(), b.size()); + return false; + } + + // Calculate A+ (pseudoinverse of A) = V S+ U*, where U* is Hermition of U (just transpose if all values of U are + // real) + // in order to solve Ax=b -> x*=A+ b + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::MatrixXd& U = svd.matrixU(); + const Eigen::VectorXd& Sv = svd.singularValues(); + const Eigen::MatrixXd& V = svd.matrixV(); + + // calculate the reciprocal of Singular-Values + // damp inverse with lambda so that inverse doesn't oscillate near solution + long int nSv = Sv.size(); + Eigen::VectorXd inv_Sv(nSv); + for (long int i = 0; i < nSv; ++i) + { + if (fabs(Sv(i)) > eps) + inv_Sv(i) = 1 / Sv(i); + else + inv_Sv(i) = Sv(i) / (Sv(i) * Sv(i) + lambda * lambda); + } + x = V * inv_Sv.asDiagonal() * U.transpose() * b; + return true; +} + +bool dampedPInv(const Eigen::Ref& A, + Eigen::Ref P, + const double eps, + const double lambda) +{ + if ((A.rows() == 0) || (A.cols() == 0)) + { + CONSOLE_BRIDGE_logError("Empty matrices not supported in dampedPInv()"); + return false; + } + + // Calculate A+ (pseudoinverse of A) = V S+ U*, where U* is Hermition of U (just transpose if all values of U are + // real) + // in order to solve Ax=b -> x*=A+ b + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::MatrixXd& U = svd.matrixU(); + const Eigen::VectorXd& Sv = svd.singularValues(); + const Eigen::MatrixXd& V = svd.matrixV(); + + // calculate the reciprocal of Singular-Values + // damp inverse with lambda so that inverse doesn't oscillate near solution + long int nSv = Sv.size(); + Eigen::VectorXd inv_Sv(nSv); + for (long int i = 0; i < nSv; ++i) + { + if (fabs(Sv(i)) > eps) + inv_Sv(i) = 1 / Sv(i); + else + { + inv_Sv(i) = Sv(i) / (Sv(i) * Sv(i) + lambda * lambda); + } + } + P = V * inv_Sv.asDiagonal() * U.transpose(); + return true; +} + +bool isNearSingularity(const Eigen::Ref& jacobian, double threshold) +{ + Eigen::JacobiSVD svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::VectorXd& sv = svd.singularValues(); + return (sv.tail(1).value() < threshold); +} + +Manipulability calcManipulability(const Eigen::Ref& jacobian) +{ + Manipulability manip; + Eigen::MatrixXd jacob_linear = jacobian.topRows(3); + Eigen::MatrixXd jacob_angular = jacobian.bottomRows(3); + + auto fn = [](const Eigen::MatrixXd& m) { + ManipulabilityEllipsoid data; + Eigen::SelfAdjointEigenSolver sm(m, Eigen::DecompositionOptions::EigenvaluesOnly); + data.eigen_values = sm.eigenvalues().real(); + + // Set eigenvalues near zero to zero. This also implies zero volume + for (Eigen::Index i = 0; i < data.eigen_values.size(); ++i) + { + if (tesseract_common::almostEqualRelativeAndAbs(data.eigen_values[i], 0)) + data.eigen_values[i] = +0; + } + + // If the minimum eigen value is approximately zero set measure and condition to max double + if (tesseract_common::almostEqualRelativeAndAbs(data.eigen_values.minCoeff(), 0)) + { + data.measure = std::numeric_limits::max(); + data.condition = std::numeric_limits::max(); + } + else + { + data.condition = data.eigen_values.maxCoeff() / data.eigen_values.minCoeff(); + data.measure = std::sqrt(data.condition); + } + + data.volume = std::sqrt(data.eigen_values.prod()); + + return data; + }; + + Eigen::MatrixXd a = jacobian * jacobian.transpose(); + Eigen::MatrixXd a_linear = jacob_linear * jacob_linear.transpose(); + Eigen::MatrixXd a_angular = jacob_angular * jacob_angular.transpose(); + manip.m = fn(a); + manip.m_linear = fn(a_linear); + manip.m_angular = fn(a_angular); + + Eigen::MatrixXd a_inv = a.inverse(); + Eigen::MatrixXd a_linear_inv = a_linear.inverse(); + Eigen::MatrixXd a_angular_inv = a_angular.inverse(); + manip.f = fn(a_inv); + manip.f_linear = fn(a_linear_inv); + manip.f_angular = fn(a_angular_inv); + + return manip; +} +} // namespace tesseract_kinematics diff --git a/tesseract_kinematics/core/src/validate.cpp b/tesseract_kinematics/core/src/validate.cpp index a78289e6967..d0e951a19d4 100644 --- a/tesseract_kinematics/core/src/validate.cpp +++ b/tesseract_kinematics/core/src/validate.cpp @@ -31,8 +31,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/ikfast/include/tesseract_kinematics/ikfast/ikfast_factory_boilerplate.h b/tesseract_kinematics/ikfast/include/tesseract_kinematics/ikfast/ikfast_factory_boilerplate.h index ca99c0f42be..3e76cf75e79 100644 --- a/tesseract_kinematics/ikfast/include/tesseract_kinematics/ikfast/ikfast_factory_boilerplate.h +++ b/tesseract_kinematics/ikfast/include/tesseract_kinematics/ikfast/ikfast_factory_boilerplate.h @@ -28,6 +28,7 @@ #include #include +#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h index 0ef213c6bfc..481706fec5b 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h @@ -32,38 +32,38 @@ namespace tesseract_kinematics { class KDLFwdKinChainFactory : public FwdKinFactory { - ForwardKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; class KDLInvKinChainLMAFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; class KDLInvKinChainNRFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; class KDLInvKinChainNR_JLFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(KDLFactoriesAnchor) diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h index 7355fa3524d..be0672d68dd 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h @@ -28,15 +28,10 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include #include #include #include -#include #include - -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h index 3a5839c3821..01fa61d3c72 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h @@ -27,18 +27,12 @@ #define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_LMA_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include #include #include -#include #include - -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include #include namespace tesseract_kinematics diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h index 5bf6d082c31..025e0b03435 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h @@ -27,16 +27,11 @@ #define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include #include #include #include #include -#include #include - -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h index e56e05b0325..8e9937cdaaf 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h @@ -27,17 +27,14 @@ #define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h index a7f24468b2b..5b6ca20e705 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h @@ -30,15 +30,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include -#include -#include -#include +#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/kdl/src/kdl_factories.cpp b/tesseract_kinematics/kdl/src/kdl_factories.cpp index 75598c84ee1..020bb6ea744 100644 --- a/tesseract_kinematics/kdl/src/kdl_factories.cpp +++ b/tesseract_kinematics/kdl/src/kdl_factories.cpp @@ -30,13 +30,17 @@ #include #include +#include +#include + namespace tesseract_kinematics { -ForwardKinematics::UPtr KDLFwdKinChainFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& /*scene_state*/, - const KinematicsPluginFactory& /*plugin_factory*/, - const YAML::Node& config) const +std::unique_ptr +KDLFwdKinChainFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const { std::string base_link; std::string tip_link; @@ -62,11 +66,12 @@ ForwardKinematics::UPtr KDLFwdKinChainFactory::create(const std::string& solver_ return std::make_unique(scene_graph, base_link, tip_link, solver_name); } -InverseKinematics::UPtr KDLInvKinChainLMAFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& /*scene_state*/, - const KinematicsPluginFactory& /*plugin_factory*/, - const YAML::Node& config) const +std::unique_ptr +KDLInvKinChainLMAFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const { std::string base_link; std::string tip_link; @@ -92,11 +97,12 @@ InverseKinematics::UPtr KDLInvKinChainLMAFactory::create(const std::string& solv return std::make_unique(scene_graph, base_link, tip_link, solver_name); } -InverseKinematics::UPtr KDLInvKinChainNRFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& /*scene_state*/, - const KinematicsPluginFactory& /*plugin_factory*/, - const YAML::Node& config) const +std::unique_ptr +KDLInvKinChainNRFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const { std::string base_link; std::string tip_link; @@ -122,11 +128,12 @@ InverseKinematics::UPtr KDLInvKinChainNRFactory::create(const std::string& solve return std::make_unique(scene_graph, base_link, tip_link, solver_name); } -InverseKinematics::UPtr KDLInvKinChainNR_JLFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& /*scene_state*/, - const KinematicsPluginFactory& /*plugin_factory*/, - const YAML::Node& config) const +std::unique_ptr +KDLInvKinChainNR_JLFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const { std::string base_link; std::string tip_link; diff --git a/tesseract_kinematics/kdl/src/kdl_fwd_kin_chain.cpp b/tesseract_kinematics/kdl/src/kdl_fwd_kin_chain.cpp index 077c875fb9d..345e18a9588 100644 --- a/tesseract_kinematics/kdl/src/kdl_fwd_kin_chain.cpp +++ b/tesseract_kinematics/kdl/src/kdl_fwd_kin_chain.cpp @@ -25,7 +25,8 @@ */ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include +#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp index 5b202b20d44..23c73e5076e 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp @@ -25,14 +25,13 @@ */ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include +#include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include -#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp index 3d268e64d95..223186ab720 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp @@ -25,14 +25,13 @@ */ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include +#include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include -#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp index 47cdfa92eb8..4b01995a97b 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp @@ -25,14 +25,13 @@ */ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include +#include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include -#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/kdl/src/kdl_utils.cpp b/tesseract_kinematics/kdl/src/kdl_utils.cpp index ff4a47dc22f..bbb8bcf98bc 100644 --- a/tesseract_kinematics/kdl/src/kdl_utils.cpp +++ b/tesseract_kinematics/kdl/src/kdl_utils.cpp @@ -24,7 +24,15 @@ * limitations under the License. */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + #include +#include +#include +#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_factory.h b/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_factory.h index 1e268751d9d..de226208b67 100644 --- a/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_factory.h +++ b/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_factory.h @@ -32,11 +32,11 @@ namespace tesseract_kinematics { class OPWInvKinFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(OPWFactoriesAnchor) diff --git a/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_inv_kin.h b/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_inv_kin.h index 452d2a3bc67..d2282115715 100644 --- a/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_inv_kin.h +++ b/tesseract_kinematics/opw/include/tesseract_kinematics/opw/opw_inv_kin.h @@ -32,7 +32,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include namespace tesseract_kinematics { diff --git a/tesseract_kinematics/opw/src/opw_factory.cpp b/tesseract_kinematics/opw/src/opw_factory.cpp index eadba3681e4..04e619d0d27 100644 --- a/tesseract_kinematics/opw/src/opw_factory.cpp +++ b/tesseract_kinematics/opw/src/opw_factory.cpp @@ -27,13 +27,16 @@ #include #include +#include +#include + namespace tesseract_kinematics { -InverseKinematics::UPtr OPWInvKinFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& /*scene_state*/, - const KinematicsPluginFactory& /*plugin_factory*/, - const YAML::Node& config) const +std::unique_ptr OPWInvKinFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const { std::string base_link; std::string tip_link; diff --git a/tesseract_kinematics/test/kinematics_test_utils.h b/tesseract_kinematics/test/kinematics_test_utils.h index bbea390c3e0..c14f7e168a9 100644 --- a/tesseract_kinematics/test/kinematics_test_utils.h +++ b/tesseract_kinematics/test/kinematics_test_utils.h @@ -42,6 +42,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include #include #include diff --git a/tesseract_kinematics/ur/include/tesseract_kinematics/ur/ur_factory.h b/tesseract_kinematics/ur/include/tesseract_kinematics/ur/ur_factory.h index 47e53ee11fe..d4d08a4102d 100644 --- a/tesseract_kinematics/ur/include/tesseract_kinematics/ur/ur_factory.h +++ b/tesseract_kinematics/ur/include/tesseract_kinematics/ur/ur_factory.h @@ -25,17 +25,18 @@ */ #ifndef TESSERACT_KINEMATICS_UR_FACTORY_H #define TESSERACT_KINEMATICS_UR_FACTORY_H + #include namespace tesseract_kinematics { class URInvKinFactory : public InvKinFactory { - InverseKinematics::UPtr create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& scene_state, - const KinematicsPluginFactory& plugin_factory, - const YAML::Node& config) const override final; + std::unique_ptr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; }; TESSERACT_PLUGIN_ANCHOR_DECL(URFactoriesAnchor) diff --git a/tesseract_kinematics/ur/src/ur_factory.cpp b/tesseract_kinematics/ur/src/ur_factory.cpp index 46c6ce861f6..9864a372697 100644 --- a/tesseract_kinematics/ur/src/ur_factory.cpp +++ b/tesseract_kinematics/ur/src/ur_factory.cpp @@ -33,13 +33,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include + namespace tesseract_kinematics { -InverseKinematics::UPtr URInvKinFactory::create(const std::string& solver_name, - const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_scene_graph::SceneState& /*scene_state*/, - const KinematicsPluginFactory& /*plugin_factory*/, - const YAML::Node& config) const +std::unique_ptr URInvKinFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const { std::string base_link; std::string tip_link; diff --git a/tesseract_scene_graph/examples/build_scene_graph_example.cpp b/tesseract_scene_graph/examples/build_scene_graph_example.cpp index 579aef12c49..1b01d6466a7 100644 --- a/tesseract_scene_graph/examples/build_scene_graph_example.cpp +++ b/tesseract_scene_graph/examples/build_scene_graph_example.cpp @@ -4,6 +4,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include #include using namespace tesseract_scene_graph; diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/fwd.h b/tesseract_scene_graph/include/tesseract_scene_graph/fwd.h new file mode 100644 index 00000000000..c22a38b22de --- /dev/null +++ b/tesseract_scene_graph/include/tesseract_scene_graph/fwd.h @@ -0,0 +1,56 @@ +/** + * @file fwd.h + * @brief Tesseract Scene Graph Forward Declarations + * + * @author Levi Armstrong + * @date February 18, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_SCENE_GRAPH_FWD_H +#define TESSERACT_SCENE_GRAPH_FWD_H + +namespace tesseract_scene_graph +{ +// joint.h +class JointDynamics; +class JointLimits; +class JointSafety; +class JointCalibration; +class JointMimic; +enum class JointType; +class Joint; + +// link.h +class Material; +class Inertial; +class Visual; +class Collision; +class Link; + +// graph.h +struct ShortestPath; +class SceneGraph; + +// scene_state.h +struct SceneState; +} // namespace tesseract_scene_graph + +#endif // TESSERACT_SCENE_GRAPH_FWD_H diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h index 29b44f490a4..f6ff5c435d1 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h @@ -28,19 +28,15 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include #include // for customizable graphs -#include // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph #include -#include -#include -#include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include +#include #ifndef SWIG @@ -79,6 +75,10 @@ BOOST_INSTALL_PROPERTY(graph, root); namespace tesseract_scene_graph { +class Link; +class Joint; +class JointLimits; + #ifndef SWIG /** @brief Defines the boost graph property. */ @@ -88,7 +88,7 @@ using GraphProperty = /** @brief Defines the boost graph vertex property. */ using VertexProperty = boost::property< boost::vertex_link_t, - Link::Ptr, + std::shared_ptr, boost::property>>; /** @@ -96,7 +96,8 @@ using VertexProperty = boost::property< * * The edge_weight represents the distance between the two links */ -using EdgeProperty = boost::property>; +using EdgeProperty = + boost::property, boost::property>; using Graph = boost:: adjacency_list; @@ -201,19 +202,19 @@ class SceneGraph * @param name The name of the link * @return Return nullptr if link name does not exists, otherwise a pointer to the link */ - Link::ConstPtr getLink(const std::string& name) const; + std::shared_ptr getLink(const std::string& name) const; /** * @brief Get a vector links in the scene graph * @return A vector of links */ - std::vector getLinks() const; + std::vector> getLinks() const; /** * @brief Get a vector leaf links in the scene graph * @return A vector of links */ - std::vector getLeafLinks() const; + std::vector> getLeafLinks() const; /** * @brief Removes a link from the graph @@ -271,7 +272,7 @@ class SceneGraph * @param name The name of the joint * @return Return nullptr if joint name does not exists, otherwise a pointer to the joint */ - Joint::ConstPtr getJoint(const std::string& name) const; + std::shared_ptr getJoint(const std::string& name) const; /** * @brief Removes a joint from the graph @@ -293,13 +294,13 @@ class SceneGraph * @brief Get a vector of joints in the scene graph * @return A vector of joints */ - std::vector getJoints() const; + std::vector> getJoints() const; /** * @brief Get a vector of active joints in the scene graph * @return A vector of active joints */ - std::vector getActiveJoints() const; + std::vector> getActiveJoints() const; /** @brief Changes the "origin" transform of the joint and recomputes the associated edge * @param name Name of the joint to be changed @@ -345,13 +346,13 @@ class SceneGraph * @param name Name of the joint which limits will be retrieved * @return Limits of the joint. Returns nullptr is joint is not found. */ - JointLimits::ConstPtr getJointLimits(const std::string& name); + std::shared_ptr getJointLimits(const std::string& name); /** * @brief Set the allowed collision matrix * @param acm The allowed collision matrix to assign */ - void setAllowedCollisionMatrix(tesseract_common::AllowedCollisionMatrix::Ptr acm); + void setAllowedCollisionMatrix(std::shared_ptr acm); /** * @brief Disable collision between two collision objects @@ -389,27 +390,27 @@ class SceneGraph * @brief Get the allowed collision matrix * @return AllowedCollisionMatrixConstPtr */ - tesseract_common::AllowedCollisionMatrix::ConstPtr getAllowedCollisionMatrix() const; + std::shared_ptr getAllowedCollisionMatrix() const; /** * @brief Get the allowed collision matrix * @return AllowedCollisionMatrixPtr */ - tesseract_common::AllowedCollisionMatrix::Ptr getAllowedCollisionMatrix(); + std::shared_ptr getAllowedCollisionMatrix(); /** * @brief Get the source link (parent link) for a joint * @param joint_name The name of the joint * @return The source link */ - Link::ConstPtr getSourceLink(const std::string& joint_name) const; + std::shared_ptr getSourceLink(const std::string& joint_name) const; /** * @brief Get the target link (child link) for a joint * @param joint_name The name of the joint * @return The target link */ - Link::ConstPtr getTargetLink(const std::string& joint_name) const; + std::shared_ptr getTargetLink(const std::string& joint_name) const; /** * @brief Get inbound joints for a link @@ -420,7 +421,7 @@ class SceneGraph * @param link_name The name of the link * @return Vector of joints */ - std::vector getInboundJoints(const std::string& link_name) const; + std::vector> getInboundJoints(const std::string& link_name) const; /** * @brief Get outbound joints for a link @@ -431,7 +432,7 @@ class SceneGraph * @param link_name The name of the link * @return Vector of joints */ - std::vector getOutboundJoints(const std::string& link_name) const; + std::vector> getOutboundJoints(const std::string& link_name) const; /** * @brief Determine if the graph contains cycles @@ -563,7 +564,7 @@ class SceneGraph * @param replace_allowed If true and the link exist it will be replaced * @return Return False if a link with the same name already exists and replace is not allowed, otherwise true */ - bool addLinkHelper(const Link::Ptr& link_ptr, bool replace_allowed = false); + bool addLinkHelper(const std::shared_ptr& link_ptr, bool replace_allowed = false); /** * @brief Adds joint to the graph @@ -571,126 +572,16 @@ class SceneGraph * @return Return False if parent or child link does not exists and if joint name already exists in the graph, * otherwise true */ - bool addJointHelper(const Joint::Ptr& joint_ptr); + bool addJointHelper(const std::shared_ptr& joint_ptr); private: - std::unordered_map> link_map_; - std::unordered_map> joint_map_; - tesseract_common::AllowedCollisionMatrix::Ptr acm_; + std::unordered_map, Vertex>> link_map_; + std::unordered_map, Edge>> joint_map_; + std::shared_ptr acm_; /** @brief The rebuild the link and joint map by extraction information from the graph */ void rebuildLinkAndJointMaps(); - struct cycle_detector : public boost::dfs_visitor<> - { - cycle_detector(bool& ascyclic) : ascyclic_(ascyclic) {} - - template - void back_edge(e, g&) - { - ascyclic_ = false; - } - - protected: - bool& ascyclic_; - }; - - struct tree_detector : public boost::dfs_visitor<> - { - tree_detector(bool& tree) : tree_(tree) {} - - template - void discover_vertex(u vertex, const g& graph) - { - auto num_in_edges = static_cast(boost::in_degree(vertex, graph)); - - if (num_in_edges > 1) - { - tree_ = false; - return; - } - - // Check if more that one root exist - if (num_in_edges == 0 && found_root_) - { - tree_ = false; - return; - } - - if (num_in_edges == 0) - found_root_ = true; - - // Check if not vertex is unused. - if (num_in_edges == 0 && boost::out_degree(vertex, graph) == 0) - { - tree_ = false; - return; - } - } - - template - void back_edge(e, const g&) - { - tree_ = false; - } - - protected: - bool& tree_; - bool found_root_{ false }; - }; - - struct children_detector : public boost::default_bfs_visitor - { - children_detector(std::vector& children) : children_(children) {} - - template - void discover_vertex(u vertex, const g& graph) - { - children_.push_back(boost::get(boost::vertex_link, graph)[vertex]->getName()); - } - - protected: - std::vector& children_; - }; - - struct adjacency_detector : public boost::default_bfs_visitor - { - adjacency_detector(std::unordered_map& adjacency_map, - std::map& color_map, - const std::string& base_link_name, - const std::vector& terminate_on_links) - : adjacency_map_(adjacency_map) - , color_map_(color_map) - , base_link_name_(base_link_name) - , terminate_on_links_(terminate_on_links) - { - } - - template - void examine_vertex(u vertex, const g& graph) - { - for (auto vd : boost::make_iterator_range(adjacent_vertices(vertex, graph))) - { - std::string adj_link = boost::get(boost::vertex_link, graph)[vd]->getName(); - if (std::find(terminate_on_links_.begin(), terminate_on_links_.end(), adj_link) != terminate_on_links_.end()) - color_map_[vd] = boost::default_color_type::black_color; - } - } - - template - void discover_vertex(u vertex, const g& graph) - { - std::string adj_link = boost::get(boost::vertex_link, graph)[vertex]->getName(); - adjacency_map_[adj_link] = base_link_name_; - } - - protected: - std::unordered_map& adjacency_map_; - std::map& color_map_; - const std::string& base_link_name_; - const std::vector& terminate_on_links_; - }; - /** * @brief Get the children of a vertex starting with start_vertex * @@ -699,31 +590,7 @@ class SceneGraph * @param start_vertex The vertex to find childeren for. * @return A list of child link names including the start vertex */ - std::vector getLinkChildrenHelper(Vertex start_vertex) const - { - const auto& graph = static_cast(*this); - std::vector child_link_names; - - std::map index_map; - boost::associative_property_map> prop_index_map(index_map); - - std::map color_map; - boost::associative_property_map> prop_color_map(color_map); - - int c = 0; - Graph::vertex_iterator i, iend; - for (boost::tie(i, iend) = boost::vertices(graph); i != iend; ++i, ++c) - boost::put(prop_index_map, *i, c); - - children_detector vis(child_link_names); - // NOLINTNEXTLINE - boost::breadth_first_search( - graph, - start_vertex, - boost::visitor(vis).root_vertex(start_vertex).vertex_index_map(prop_index_map).color_map(prop_color_map)); - - return child_link_names; - } + std::vector getLinkChildrenHelper(Vertex start_vertex) const; friend class boost::serialization::access; template @@ -736,26 +603,11 @@ class SceneGraph void serialize(Archive& ar, const unsigned int version); // NOLINT }; -inline std::ostream& operator<<(std::ostream& os, const ShortestPath& path) -{ - os << "Links:" << std::endl; - for (const auto& l : path.links) - os << " " << l << std::endl; - - os << "Joints:" << std::endl; - for (const auto& j : path.joints) - os << " " << j << std::endl; - - os << "Active Joints:" << std::endl; - for (const auto& j : path.active_joints) - os << " " << j << std::endl; - return os; -} +std::ostream& operator<<(std::ostream& os, const ShortestPath& path); } // namespace tesseract_scene_graph #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::SceneGraph, "SceneGraph") #endif // TESSERACT_SCENE_GRAPH_GRAPH_H diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/joint.h b/tesseract_scene_graph/include/tesseract_scene_graph/joint.h index b4cfe37b632..ff7b7b31a84 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/joint.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/joint.h @@ -42,6 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -56,15 +57,13 @@ class JointDynamics using ConstPtr = std::shared_ptr; JointDynamics() = default; - JointDynamics(double damping, double friction) : damping(damping), friction(friction) {} + JointDynamics(double damping, double friction); + double damping{ 0 }; double friction{ 0 }; - void clear() - { - damping = 0; - friction = 0; - } + void clear(); + bool operator==(const JointDynamics& rhs) const; bool operator!=(const JointDynamics& rhs) const; @@ -74,6 +73,8 @@ class JointDynamics void serialize(Archive& ar, const unsigned int version); // NOLINT }; +std::ostream& operator<<(std::ostream& os, const JointDynamics& dynamics); + class JointLimits { public: @@ -81,10 +82,7 @@ class JointLimits using ConstPtr = std::shared_ptr; JointLimits() = default; - JointLimits(double l, double u, double e, double v, double a) - : lower(l), upper(u), effort(e), velocity(v), acceleration(a) - { - } + JointLimits(double l, double u, double e, double v, double a); double lower{ 0 }; double upper{ 0 }; @@ -92,21 +90,8 @@ class JointLimits double velocity{ 0 }; double acceleration{ 0 }; - void clear() - { - lower = 0; - upper = 0; - effort = 0; - velocity = 0; - acceleration = 0; - } - - friend std::ostream& operator<<(std::ostream& os, const JointLimits& limits) - { - os << "lower=" << limits.lower << " upper=" << limits.upper << " effort=" << limits.effort - << " velocity=" << limits.velocity << " acceleration=" << limits.acceleration; - return os; - }; + void clear(); + bool operator==(const JointLimits& rhs) const; bool operator!=(const JointLimits& rhs) const; @@ -116,6 +101,8 @@ class JointLimits void serialize(Archive& ar, const unsigned int version); // NOLINT }; +std::ostream& operator<<(std::ostream& os, const JointLimits& limits); + /// \brief Parameters for Joint Safety Controllers class JointSafety { @@ -124,13 +111,7 @@ class JointSafety using ConstPtr = std::shared_ptr; JointSafety() = default; - JointSafety(double soft_upper_limit, double soft_lower_limit, double k_position, double k_velocity) - : soft_upper_limit(soft_upper_limit) - , soft_lower_limit(soft_lower_limit) - , k_position(k_position) - , k_velocity(k_velocity) - { - } + JointSafety(double soft_upper_limit, double soft_lower_limit, double k_position, double k_velocity); /// /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. @@ -169,29 +150,19 @@ class JointSafety double k_position{ 0 }; double k_velocity{ 0 }; - void clear() - { - soft_upper_limit = 0; - soft_lower_limit = 0; - k_position = 0; - k_velocity = 0; - } + void clear(); + bool operator==(const JointSafety& rhs) const; bool operator!=(const JointSafety& rhs) const; - friend std::ostream& operator<<(std::ostream& os, const JointSafety& safety) - { - os << "soft_upper_limit=" << safety.soft_upper_limit << " soft_lower_limit=" << safety.soft_lower_limit - << " k_position=" << safety.k_position << " k_velocity=" << safety.k_velocity; - return os; - }; - private: friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT }; +std::ostream& operator<<(std::ostream& os, const JointSafety& safety); + class JointCalibration { public: @@ -199,36 +170,25 @@ class JointCalibration using ConstPtr = std::shared_ptr; JointCalibration() = default; - JointCalibration(double reference_position, double rising, double falling) - : reference_position(reference_position), rising(rising), falling(falling) - { - } + JointCalibration(double reference_position, double rising, double falling); + double reference_position{ 0 }; double rising{ 0 }; double falling{ 0 }; - void clear() - { - reference_position = 0; - rising = 0; - falling = 0; - } + void clear(); + bool operator==(const JointCalibration& rhs) const; bool operator!=(const JointCalibration& rhs) const; - friend std::ostream& operator<<(std::ostream& os, const JointCalibration& calibration) - { - os << "reference_position=" << calibration.reference_position << " rising=" << calibration.rising - << " falling=" << calibration.falling; - return os; - }; - private: friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT }; +std::ostream& operator<<(std::ostream& os, const JointCalibration& calibration); + class JointMimic { public: @@ -236,35 +196,25 @@ class JointMimic using ConstPtr = std::shared_ptr; JointMimic() = default; - JointMimic(double offset, double multiplier, std::string joint_name) - : offset(offset), multiplier(multiplier), joint_name(std::move(joint_name)) - { - } + JointMimic(double offset, double multiplier, std::string joint_name); + double offset{ 0 }; double multiplier{ 1.0 }; std::string joint_name; - void clear() - { - offset = 0.0; - multiplier = 1.0; - joint_name.clear(); - } + void clear(); + bool operator==(const JointMimic& rhs) const; bool operator!=(const JointMimic& rhs) const; - friend std::ostream& operator<<(std::ostream& os, const JointMimic& mimic) - { - os << "joint_name=" << mimic.joint_name << " offset=" << mimic.offset << " multiplier=" << mimic.multiplier; - return os; - }; - private: friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT }; +std::ostream& operator<<(std::ostream& os, const JointMimic& mimic); + enum class JointType { UNKNOWN, @@ -284,7 +234,7 @@ class Joint using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Joint(std::string name) : name_(std::move(name)) { this->clear(); } + Joint(std::string name); Joint() = default; ~Joint() = default; // Joints are non-copyable as their name must be unique @@ -294,7 +244,7 @@ class Joint Joint(Joint&& other) = default; Joint& operator=(Joint&& other) = default; - const std::string& getName() const { return name_; } + const std::string& getName() const; /// The type of joint JointType type{ JointType::UNKNOWN }; @@ -335,58 +285,18 @@ class Joint /// Option to Mimic another Joint JointMimic::Ptr mimic; - void clear() - { - this->axis = Eigen::Vector3d(1, 0, 0); - this->child_link_name.clear(); - this->parent_link_name.clear(); - this->parent_to_joint_origin_transform.setIdentity(); - this->dynamics.reset(); - this->limits.reset(); - this->safety.reset(); - this->calibration.reset(); - this->mimic.reset(); - this->type = JointType::UNKNOWN; - } + void clear(); /** * @brief Clone the joint keeping the name * @return Cloned joint */ - Joint clone() const { return clone(name_); } + Joint clone() const; /* Create a clone of current joint, with a new name. Child link name and parent link name are unchanged. * All underlying properties, such as dynamics, limits... are copied as well.*/ - Joint clone(const std::string& name) const - { - Joint ret(name); - ret.axis = this->axis; - ret.child_link_name = this->child_link_name; - ret.parent_link_name = this->parent_link_name; - ret.parent_to_joint_origin_transform = this->parent_to_joint_origin_transform; - ret.type = this->type; - if (this->dynamics) - { - ret.dynamics = std::make_shared(*(this->dynamics)); - } - if (this->limits) - { - ret.limits = std::make_shared(*(this->limits)); - } - if (this->safety) - { - ret.safety = std::make_shared(*(this->safety)); - } - if (this->calibration) - { - ret.calibration = std::make_shared(*(this->calibration)); - } - if (this->mimic) - { - ret.mimic = std::make_shared(*(this->mimic)); - } - return ret; - } + Joint clone(const std::string& name) const; + bool operator==(const Joint& rhs) const; bool operator!=(const Joint& rhs) const; @@ -398,48 +308,8 @@ class Joint void serialize(Archive& ar, const unsigned int version); // NOLINT }; -inline std::ostream& operator<<(std::ostream& os, const JointType& type) -{ - switch (type) - { - case JointType::FIXED: - { - os << "Fixed"; - break; - } - case JointType::PLANAR: - { - os << "Planar"; - break; - } - case JointType::FLOATING: - { - os << "Floating"; - break; - } - case JointType::REVOLUTE: - { - os << "Revolute"; - break; - } - case JointType::PRISMATIC: - { - os << "Prismatic"; - break; - } - case JointType::CONTINUOUS: - { - os << "Continuous"; - break; - } - default: - { - os << "Unknown"; - break; - } - } - return os; -} +std::ostream& operator<<(std::ostream& os, const JointType& type); + } // namespace tesseract_scene_graph #include diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h b/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h index 4db40cdd4d3..f669bb076cf 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h @@ -41,16 +41,21 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +// #include namespace tesseract_scene_graph { +class SceneGraph; +class Joint; +class Inertial; + /** * @brief Convert Eigen::Isometry3d to KDL::Frame * @param transform Input Eigen transform (Isometry3d) @@ -106,14 +111,14 @@ Eigen::MatrixXd convert(const KDL::Jacobian& jacobian, const std::vector& q * @param joint Tesseract Joint * @return A KDL Joint */ -KDL::Joint convert(const Joint::ConstPtr& joint); +KDL::Joint convert(const std::shared_ptr& joint); /** * @brief Convert Tesseract Inertail to KDL Inertial * @param inertial * @return */ -KDL::RigidBodyInertia convert(const Inertial::ConstPtr& inertial); +KDL::RigidBodyInertia convert(const std::shared_ptr& inertial); /** @brief The KDLTreeData populated when parsing scene graph */ struct KDLTreeData diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/link.h b/tesseract_scene_graph/include/tesseract_scene_graph/link.h index cd2a7dffefa..3b163628406 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/link.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/link.h @@ -46,8 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include namespace tesseract_scene_graph { @@ -60,18 +59,15 @@ class Material using ConstPtr = std::shared_ptr; Material() = default; - Material(std::string name) : name_(std::move(name)) { this->clear(); } + explicit Material(std::string name); - const std::string& getName() const { return name_; } + const std::string& getName() const; std::string texture_filename; Eigen::Vector4d color; - void clear() - { - color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0); - texture_filename.clear(); - } + void clear(); + bool operator==(const Material& rhs) const; bool operator!=(const Material& rhs) const; @@ -105,12 +101,8 @@ class Inertial double iyz{ 0 }; double izz{ 0 }; - void clear() - { - origin.setIdentity(); - mass = 0; - ixx = ixy = ixz = iyy = iyz = izz = 0; - } + void clear(); + bool operator==(const Inertial& rhs) const; bool operator!=(const Inertial& rhs) const; @@ -128,22 +120,15 @@ class Visual using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Visual() { this->clear(); } + Visual(); Eigen::Isometry3d origin; - tesseract_geometry::Geometry::ConstPtr geometry; + std::shared_ptr geometry; Material::Ptr material; - - void clear() - { - origin.setIdentity(); - material = DEFAULT_TESSERACT_MATERIAL; - geometry.reset(); - name.clear(); - } - std::string name; + void clear(); + bool operator==(const Visual& rhs) const; bool operator!=(const Visual& rhs) const; @@ -161,19 +146,13 @@ class Collision using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Collision() { this->clear(); } + Collision(); Eigen::Isometry3d origin; - tesseract_geometry::Geometry::ConstPtr geometry; - - void clear() - { - origin.setIdentity(); - geometry.reset(); - name.clear(); - } - + std::shared_ptr geometry; std::string name; + void clear(); + bool operator==(const Collision& rhs) const; bool operator!=(const Collision& rhs) const; @@ -189,7 +168,7 @@ class Link using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Link(std::string name) : name_(std::move(name)) { this->clear(); } + Link(std::string name); Link() = default; ~Link() = default; // Links are non-copyable as their name must be unique @@ -199,7 +178,7 @@ class Link Link(Link&& other) = default; Link& operator=(Link&& other) = default; - const std::string& getName() const { return name_; } + const std::string& getName() const; /// inertial element Inertial::Ptr inertial; @@ -210,12 +189,7 @@ class Link /// Collision Elements std::vector collision; - void clear() - { - this->inertial.reset(); - this->collision.clear(); - this->visual.clear(); - } + void clear(); bool operator==(const Link& rhs) const; bool operator!=(const Link& rhs) const; @@ -224,26 +198,10 @@ class Link * @brief Clone the link keeping the name. * @return Cloned link */ - Link clone() const { return clone(name_); } + Link clone() const; /** Perform a copy of link, changing its name **/ - Link clone(const std::string& name) const - { - Link ret(name); - if (this->inertial) - { - ret.inertial = std::make_shared(*(this->inertial)); - } - for (const auto& c : this->collision) - { - ret.collision.push_back(std::make_shared(*c)); - } - for (const auto& v : this->visual) - { - ret.visual.push_back(std::make_shared(*v)); - } - return ret; - } + Link clone(const std::string& name) const; private: std::string name_; @@ -255,7 +213,6 @@ class Link } // namespace tesseract_scene_graph #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::Material, "Material") BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::Inertial, "Inertial") BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::Visual, "Visual") diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h b/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h index cb8fc20fc75..0c390827556 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/scene_state.h @@ -39,7 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_scene_graph { @@ -83,6 +83,5 @@ struct SceneState } // namespace tesseract_scene_graph #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::SceneState, "SceneState") #endif // TESSERACT_SCENE_GRAPH_SCENE_STATE_H diff --git a/tesseract_scene_graph/src/graph.cpp b/tesseract_scene_graph/src/graph.cpp index dc31f0dd072..28e4840dd59 100644 --- a/tesseract_scene_graph/src/graph.cpp +++ b/tesseract_scene_graph/src/graph.cpp @@ -26,10 +26,11 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph +#include +#include #include #include -#include -#include #include #include #include @@ -37,6 +38,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #if (BOOST_VERSION >= 107400) && (BOOST_VERSION < 107500) #include #endif @@ -45,10 +48,123 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include +#include #include namespace tesseract_scene_graph { +struct cycle_detector : public boost::dfs_visitor<> +{ + cycle_detector(bool& ascyclic) : ascyclic_(ascyclic) {} + + template + void back_edge(e, g&) + { + ascyclic_ = false; + } + +protected: + bool& ascyclic_; +}; + +struct tree_detector : public boost::dfs_visitor<> +{ + tree_detector(bool& tree) : tree_(tree) {} + + template + void discover_vertex(u vertex, const g& graph) + { + auto num_in_edges = static_cast(boost::in_degree(vertex, graph)); + + if (num_in_edges > 1) + { + tree_ = false; + return; + } + + // Check if more that one root exist + if (num_in_edges == 0 && found_root_) + { + tree_ = false; + return; + } + + if (num_in_edges == 0) + found_root_ = true; + + // Check if not vertex is unused. + if (num_in_edges == 0 && boost::out_degree(vertex, graph) == 0) + { + tree_ = false; + return; + } + } + + template + void back_edge(e, const g&) + { + tree_ = false; + } + +protected: + bool& tree_; + bool found_root_{ false }; +}; + +struct children_detector : public boost::default_bfs_visitor +{ + children_detector(std::vector& children) : children_(children) {} + + template + void discover_vertex(u vertex, const g& graph) + { + children_.push_back(boost::get(boost::vertex_link, graph)[vertex]->getName()); + } + +protected: + std::vector& children_; +}; + +struct adjacency_detector : public boost::default_bfs_visitor +{ + adjacency_detector(std::unordered_map& adjacency_map, + std::map& color_map, + const std::string& base_link_name, + const std::vector& terminate_on_links) + : adjacency_map_(adjacency_map) + , color_map_(color_map) + , base_link_name_(base_link_name) + , terminate_on_links_(terminate_on_links) + { + } + + template + void examine_vertex(u vertex, const g& graph) + { + for (auto vd : boost::make_iterator_range(adjacent_vertices(vertex, graph))) + { + std::string adj_link = boost::get(boost::vertex_link, graph)[vd]->getName(); + if (std::find(terminate_on_links_.begin(), terminate_on_links_.end(), adj_link) != terminate_on_links_.end()) + color_map_[vd] = boost::default_color_type::black_color; + } + } + + template + void discover_vertex(u vertex, const g& graph) + { + std::string adj_link = boost::get(boost::vertex_link, graph)[vertex]->getName(); + adjacency_map_[adj_link] = base_link_name_; + } + +protected: + std::unordered_map& adjacency_map_; + std::map& color_map_; + const std::string& base_link_name_; + const std::vector& terminate_on_links_; +}; + using UGraph = boost::adjacency_list; @@ -181,7 +297,7 @@ bool SceneGraph::addLink(const Link& link, const Joint& joint) return true; } -bool SceneGraph::addLinkHelper(const Link::Ptr& link_ptr, bool replace_allowed) +bool SceneGraph::addLinkHelper(const std::shared_ptr& link_ptr, bool replace_allowed) { auto found = link_map_.find(link_ptr->getName()); bool link_exists = (found != link_map_.end()); @@ -211,7 +327,7 @@ bool SceneGraph::addLinkHelper(const Link::Ptr& link_ptr, bool replace_allowed) return true; } -Link::ConstPtr SceneGraph::getLink(const std::string& name) const +std::shared_ptr SceneGraph::getLink(const std::string& name) const { auto found = link_map_.find(name); if (found == link_map_.end()) @@ -220,7 +336,7 @@ Link::ConstPtr SceneGraph::getLink(const std::string& name) const return found->second.first; } -std::vector SceneGraph::getLinks() const +std::vector> SceneGraph::getLinks() const { std::vector links; links.reserve(link_map_.size()); @@ -230,7 +346,7 @@ std::vector SceneGraph::getLinks() const return links; } -std::vector SceneGraph::getLeafLinks() const +std::vector> SceneGraph::getLeafLinks() const { std::vector links; links.reserve(link_map_.size()); @@ -345,7 +461,7 @@ bool SceneGraph::addJoint(const Joint& joint) return addJointHelper(joint_ptr); } -bool SceneGraph::addJointHelper(const Joint::Ptr& joint_ptr) +bool SceneGraph::addJointHelper(const std::shared_ptr& joint_ptr) { auto parent = link_map_.find(joint_ptr->parent_link_name); auto child = link_map_.find(joint_ptr->child_link_name); @@ -404,7 +520,7 @@ bool SceneGraph::addJointHelper(const Joint::Ptr& joint_ptr) return true; } -Joint::ConstPtr SceneGraph::getJoint(const std::string& name) const +std::shared_ptr SceneGraph::getJoint(const std::string& name) const { auto found = joint_map_.find(name); if (found == joint_map_.end()) @@ -464,7 +580,7 @@ bool SceneGraph::moveJoint(const std::string& name, const std::string& parent_li return addJointHelper(joint); } -std::vector SceneGraph::getJoints() const +std::vector> SceneGraph::getJoints() const { std::vector joints; joints.reserve(joint_map_.size()); @@ -474,7 +590,7 @@ std::vector SceneGraph::getJoints() const return joints; } -std::vector SceneGraph::getActiveJoints() const +std::vector> SceneGraph::getActiveJoints() const { std::vector joints; joints.reserve(joint_map_.size()); @@ -610,7 +726,7 @@ bool SceneGraph::changeJointAccelerationLimits(const std::string& name, double l return true; } -JointLimits::ConstPtr SceneGraph::getJointLimits(const std::string& name) +std::shared_ptr SceneGraph::getJointLimits(const std::string& name) { auto found = joint_map_.find(name); @@ -624,7 +740,10 @@ JointLimits::ConstPtr SceneGraph::getJointLimits(const std::string& name) return found->second.first->limits; } -void SceneGraph::setAllowedCollisionMatrix(tesseract_common::AllowedCollisionMatrix::Ptr acm) { acm_ = std::move(acm); } +void SceneGraph::setAllowedCollisionMatrix(std::shared_ptr acm) +{ + acm_ = std::move(acm); +} void SceneGraph::addAllowedCollision(const std::string& link_name1, const std::string& link_name2, @@ -647,25 +766,28 @@ bool SceneGraph::isCollisionAllowed(const std::string& link_name1, const std::st return acm_->isCollisionAllowed(link_name1, link_name2); } -tesseract_common::AllowedCollisionMatrix::ConstPtr SceneGraph::getAllowedCollisionMatrix() const { return acm_; } +std::shared_ptr SceneGraph::getAllowedCollisionMatrix() const +{ + return acm_; +} -tesseract_common::AllowedCollisionMatrix::Ptr SceneGraph::getAllowedCollisionMatrix() { return acm_; } +std::shared_ptr SceneGraph::getAllowedCollisionMatrix() { return acm_; } -Link::ConstPtr SceneGraph::getSourceLink(const std::string& joint_name) const +std::shared_ptr SceneGraph::getSourceLink(const std::string& joint_name) const { Edge e = getEdge(joint_name); Vertex v = boost::source(e, *this); return boost::get(boost::vertex_link, *this)[v]; } -Link::ConstPtr SceneGraph::getTargetLink(const std::string& joint_name) const +std::shared_ptr SceneGraph::getTargetLink(const std::string& joint_name) const { Edge e = getEdge(joint_name); Vertex v = boost::target(e, *this); return boost::get(boost::vertex_link, *this)[v]; } -std::vector SceneGraph::getInboundJoints(const std::string& link_name) const +std::vector> SceneGraph::getInboundJoints(const std::string& link_name) const { std::vector joints; Vertex vertex = getVertex(link_name); @@ -685,7 +807,7 @@ std::vector SceneGraph::getInboundJoints(const std::string& lin return joints; } -std::vector SceneGraph::getOutboundJoints(const std::string& link_name) const +std::vector> SceneGraph::getOutboundJoints(const std::string& link_name) const { std::vector joints; Vertex vertex = getVertex(link_name); @@ -1119,6 +1241,32 @@ void SceneGraph::rebuildLinkAndJointMaps() } } +std::vector SceneGraph::getLinkChildrenHelper(Vertex start_vertex) const +{ + const auto& graph = static_cast(*this); + std::vector child_link_names; + + std::map index_map; + boost::associative_property_map> prop_index_map(index_map); + + std::map color_map; + boost::associative_property_map> prop_color_map(color_map); + + int c = 0; + Graph::vertex_iterator i, iend; + for (boost::tie(i, iend) = boost::vertices(graph); i != iend; ++i, ++c) + boost::put(prop_index_map, *i, c); + + children_detector vis(child_link_names); + // NOLINTNEXTLINE + boost::breadth_first_search( + graph, + start_vertex, + boost::visitor(vis).root_vertex(start_vertex).vertex_index_map(prop_index_map).color_map(prop_color_map)); + + return child_link_names; +} + bool SceneGraph::operator==(const SceneGraph& rhs) const { using namespace tesseract_common; @@ -1165,6 +1313,22 @@ void SceneGraph::serialize(Archive& ar, const unsigned int version) boost::serialization::split_member(ar, *this, version); } +std::ostream& operator<<(std::ostream& os, const ShortestPath& path) +{ + os << "Links:" << std::endl; + for (const auto& l : path.links) + os << " " << l << std::endl; + + os << "Joints:" << std::endl; + for (const auto& j : path.joints) + os << " " << j << std::endl; + + os << "Active Joints:" << std::endl; + for (const auto& j : path.active_joints) + os << " " << j << std::endl; + return os; +} + } // namespace tesseract_scene_graph #include diff --git a/tesseract_scene_graph/src/joint.cpp b/tesseract_scene_graph/src/joint.cpp index a0b7c61b655..37f77f22beb 100644 --- a/tesseract_scene_graph/src/joint.cpp +++ b/tesseract_scene_graph/src/joint.cpp @@ -27,6 +27,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -38,6 +39,14 @@ namespace tesseract_scene_graph /*********************************************************/ /****** JointDynamics *****/ /*********************************************************/ +JointDynamics::JointDynamics(double damping, double friction) : damping(damping), friction(friction) {} + +void JointDynamics::clear() +{ + damping = 0; + friction = 0; +} + bool JointDynamics::operator==(const JointDynamics& rhs) const { bool equal = true; @@ -55,9 +64,30 @@ void JointDynamics::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(friction); } +std::ostream& operator<<(std::ostream& os, const JointDynamics& dynamics) +{ + os << "damping=" << dynamics.damping << " friction=" << dynamics.friction; + return os; +} + /*********************************************************/ /****** JointLimits *****/ /*********************************************************/ + +JointLimits::JointLimits(double l, double u, double e, double v, double a) + : lower(l), upper(u), effort(e), velocity(v), acceleration(a) +{ +} + +void JointLimits::clear() +{ + lower = 0; + upper = 0; + effort = 0; + velocity = 0; + acceleration = 0; +} + bool JointLimits::operator==(const JointLimits& rhs) const { bool equal = true; @@ -81,9 +111,33 @@ void JointLimits::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(acceleration); } +std::ostream& operator<<(std::ostream& os, const JointLimits& limits) +{ + os << "lower=" << limits.lower << " upper=" << limits.upper << " effort=" << limits.effort + << " velocity=" << limits.velocity << " acceleration=" << limits.acceleration; + return os; +} + /*********************************************************/ /****** JointSafety *****/ /*********************************************************/ + +JointSafety::JointSafety(double soft_upper_limit, double soft_lower_limit, double k_position, double k_velocity) + : soft_upper_limit(soft_upper_limit) + , soft_lower_limit(soft_lower_limit) + , k_position(k_position) + , k_velocity(k_velocity) +{ +} + +void JointSafety::clear() +{ + soft_upper_limit = 0; + soft_lower_limit = 0; + k_position = 0; + k_velocity = 0; +} + bool JointSafety::operator==(const JointSafety& rhs) const { bool equal = true; @@ -105,9 +159,28 @@ void JointSafety::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(k_velocity); } +std::ostream& operator<<(std::ostream& os, const JointSafety& safety) +{ + os << "soft_upper_limit=" << safety.soft_upper_limit << " soft_lower_limit=" << safety.soft_lower_limit + << " k_position=" << safety.k_position << " k_velocity=" << safety.k_velocity; + return os; +} + /*********************************************************/ /****** JointCalibration *****/ /*********************************************************/ +JointCalibration::JointCalibration(double reference_position, double rising, double falling) + : reference_position(reference_position), rising(rising), falling(falling) +{ +} + +void JointCalibration::clear() +{ + reference_position = 0; + rising = 0; + falling = 0; +} + bool JointCalibration::operator==(const JointCalibration& rhs) const { bool equal = true; @@ -127,9 +200,28 @@ void JointCalibration::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(falling); } +std::ostream& operator<<(std::ostream& os, const JointCalibration& calibration) +{ + os << "reference_position=" << calibration.reference_position << " rising=" << calibration.rising + << " falling=" << calibration.falling; + return os; +} + /*********************************************************/ /****** JointMimic *****/ /*********************************************************/ +JointMimic::JointMimic(double offset, double multiplier, std::string joint_name) + : offset(offset), multiplier(multiplier), joint_name(std::move(joint_name)) +{ +} + +void JointMimic::clear() +{ + offset = 0.0; + multiplier = 1.0; + joint_name.clear(); +} + bool JointMimic::operator==(const JointMimic& rhs) const { bool equal = true; @@ -149,9 +241,66 @@ void JointMimic::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(joint_name); } +std::ostream& operator<<(std::ostream& os, const JointMimic& mimic) +{ + os << "joint_name=" << mimic.joint_name << " offset=" << mimic.offset << " multiplier=" << mimic.multiplier; + return os; +} + /*********************************************************/ /****** Joint *****/ /*********************************************************/ +Joint::Joint(std::string name) : name_(std::move(name)) { this->clear(); } + +const std::string& Joint::getName() const { return name_; } + +void Joint::clear() +{ + this->axis = Eigen::Vector3d(1, 0, 0); + this->child_link_name.clear(); + this->parent_link_name.clear(); + this->parent_to_joint_origin_transform.setIdentity(); + this->dynamics.reset(); + this->limits.reset(); + this->safety.reset(); + this->calibration.reset(); + this->mimic.reset(); + this->type = JointType::UNKNOWN; +} + +Joint Joint::clone() const { return clone(name_); } + +Joint Joint::clone(const std::string& name) const +{ + Joint ret(name); + ret.axis = this->axis; + ret.child_link_name = this->child_link_name; + ret.parent_link_name = this->parent_link_name; + ret.parent_to_joint_origin_transform = this->parent_to_joint_origin_transform; + ret.type = this->type; + if (this->dynamics) + { + ret.dynamics = std::make_shared(*(this->dynamics)); + } + if (this->limits) + { + ret.limits = std::make_shared(*(this->limits)); + } + if (this->safety) + { + ret.safety = std::make_shared(*(this->safety)); + } + if (this->calibration) + { + ret.calibration = std::make_shared(*(this->calibration)); + } + if (this->mimic) + { + ret.mimic = std::make_shared(*(this->mimic)); + } + return ret; +} + bool Joint::operator==(const Joint& rhs) const { bool equal = true; @@ -186,6 +335,49 @@ void Joint::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(name_); } +std::ostream& operator<<(std::ostream& os, const JointType& type) +{ + switch (type) + { + case JointType::FIXED: + { + os << "Fixed"; + break; + } + case JointType::PLANAR: + { + os << "Planar"; + break; + } + case JointType::FLOATING: + { + os << "Floating"; + break; + } + case JointType::REVOLUTE: + { + os << "Revolute"; + break; + } + case JointType::PRISMATIC: + { + os << "Prismatic"; + break; + } + case JointType::CONTINUOUS: + { + os << "Continuous"; + break; + } + default: + { + os << "Unknown"; + break; + } + } + return os; +} + } // namespace tesseract_scene_graph #include diff --git a/tesseract_scene_graph/src/kdl_parser.cpp b/tesseract_scene_graph/src/kdl_parser.cpp index 328b6b4daa5..b71fe5ce5a1 100644 --- a/tesseract_scene_graph/src/kdl_parser.cpp +++ b/tesseract_scene_graph/src/kdl_parser.cpp @@ -34,7 +34,15 @@ /* Author: Wim Meeussen & Levi Armstrong */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include +#include namespace tesseract_scene_graph { @@ -120,7 +128,7 @@ Eigen::MatrixXd convert(const KDL::Jacobian& jacobian, const std::vector& q return matrix; } -KDL::Joint convert(const Joint::ConstPtr& joint) +KDL::Joint convert(const std::shared_ptr& joint) { KDL::Frame parent_joint = convert(joint->parent_to_joint_origin_transform); const std::string& name = joint->getName(); @@ -154,7 +162,7 @@ KDL::Joint convert(const Joint::ConstPtr& joint) } } -KDL::RigidBodyInertia convert(const Inertial::ConstPtr& inertial) +KDL::RigidBodyInertia convert(const std::shared_ptr& inertial) { KDL::Frame origin = convert(inertial->origin); diff --git a/tesseract_scene_graph/src/link.cpp b/tesseract_scene_graph/src/link.cpp index facefe2d1ed..c7a82d346ce 100644 --- a/tesseract_scene_graph/src/link.cpp +++ b/tesseract_scene_graph/src/link.cpp @@ -34,12 +34,23 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_scene_graph { /*********************************************************/ /****** Material *****/ /*********************************************************/ +Material::Material(std::string name) : name_(std::move(name)) { this->clear(); } + +const std::string& Material::getName() const { return name_; } + +void Material::clear() +{ + color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0); + texture_filename.clear(); +} + bool Material::operator==(const Material& rhs) const { bool equal = true; @@ -62,6 +73,13 @@ void Material::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Inertial *****/ /*********************************************************/ +void Inertial::clear() +{ + origin.setIdentity(); + mass = 0; + ixx = ixy = ixz = iyy = iyz = izz = 0; +} + bool Inertial::operator==(const Inertial& rhs) const { bool equal = true; @@ -94,6 +112,17 @@ void Inertial::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Visual *****/ /*********************************************************/ + +Visual::Visual() { this->clear(); } + +void Visual::clear() +{ + origin.setIdentity(); + material = DEFAULT_TESSERACT_MATERIAL; + geometry.reset(); + name.clear(); +} + bool Visual::operator==(const Visual& rhs) const { bool equal = true; @@ -118,6 +147,15 @@ void Visual::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Collision *****/ /*********************************************************/ +Collision::Collision() { this->clear(); } + +void Collision::clear() +{ + origin.setIdentity(); + geometry.reset(); + name.clear(); +} + bool Collision::operator==(const Collision& rhs) const { bool equal = true; @@ -140,6 +178,37 @@ void Collision::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Link *****/ /*********************************************************/ +Link::Link(std::string name) : name_(std::move(name)) { this->clear(); } + +const std::string& Link::getName() const { return name_; } + +void Link::clear() +{ + this->inertial.reset(); + this->collision.clear(); + this->visual.clear(); +} + +Link Link::clone() const { return clone(name_); } + +Link Link::clone(const std::string& name) const +{ + Link ret(name); + if (this->inertial) + { + ret.inertial = std::make_shared(*(this->inertial)); + } + for (const auto& c : this->collision) + { + ret.collision.push_back(std::make_shared(*c)); + } + for (const auto& v : this->visual) + { + ret.visual.push_back(std::make_shared(*v)); + } + return ret; +} + bool Link::operator==(const Link& rhs) const { using namespace tesseract_common; diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_joint_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_joint_unit.cpp index e12e4ccc49d..09262635d7a 100644 --- a/tesseract_scene_graph/test/tesseract_scene_graph_joint_unit.cpp +++ b/tesseract_scene_graph/test/tesseract_scene_graph_joint_unit.cpp @@ -8,6 +8,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include TEST(TesseractSceneGraphUnit, TesseractSceneGraphJointDynamicsUnit) // NOLINT { diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_link_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_link_unit.cpp index e2bc5143e2a..d0f34d7551b 100644 --- a/tesseract_scene_graph/test/tesseract_scene_graph_link_unit.cpp +++ b/tesseract_scene_graph/test/tesseract_scene_graph_link_unit.cpp @@ -8,6 +8,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include + TEST(TesseractSceneGraphUnit, TesseractSceneGraphLinkMaterialUnit) // NOLINT { using namespace tesseract_scene_graph; diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp index cd61082f1df..ec1ca5300b7 100644 --- a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp +++ b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp @@ -11,6 +11,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include #include #include diff --git a/tesseract_srdf/CMakeLists.txt b/tesseract_srdf/CMakeLists.txt index 37fb112b915..b37841be841 100644 --- a/tesseract_srdf/CMakeLists.txt +++ b/tesseract_srdf/CMakeLists.txt @@ -88,7 +88,7 @@ configure_package( # Mark cpp header files for installation install(FILES include/${PROJECT_NAME}/srdf_model.h include/${PROJECT_NAME}/kinematics_information.h - include/${PROJECT_NAME}/utils.h DESTINATION include/${PROJECT_NAME}) + include/${PROJECT_NAME}/utils.h include/${PROJECT_NAME}/fwd.h DESTINATION include/${PROJECT_NAME}) # ADD Examples if(TESSERACT_ENABLE_EXAMPLES OR TESSERACT_SRDF_ENABLE_EXAMPLES) diff --git a/tesseract_srdf/examples/parse_srdf_example.cpp b/tesseract_srdf/examples/parse_srdf_example.cpp index 96e3aa8df89..cd0c0d06be5 100644 --- a/tesseract_srdf/examples/parse_srdf_example.cpp +++ b/tesseract_srdf/examples/parse_srdf_example.cpp @@ -1,7 +1,10 @@ #include #include +#include +#include #include #include +#include #include #include #include diff --git a/tesseract_srdf/include/tesseract_srdf/collision_margins.h b/tesseract_srdf/include/tesseract_srdf/collision_margins.h index a56156fc243..1307f406320 100644 --- a/tesseract_srdf/include/tesseract_srdf/collision_margins.h +++ b/tesseract_srdf/include/tesseract_srdf/collision_margins.h @@ -32,18 +32,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT } -namespace tesseract_scene_graph -{ -class SceneGraph; -} -namespace tesseract_common -{ -class CollisionMarginData; -} namespace tesseract_srdf { diff --git a/tesseract_srdf/include/tesseract_srdf/configs.h b/tesseract_srdf/include/tesseract_srdf/configs.h index eee91a1015f..de30293377b 100644 --- a/tesseract_srdf/include/tesseract_srdf/configs.h +++ b/tesseract_srdf/include/tesseract_srdf/configs.h @@ -31,17 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace tinyxml2 { class XMLElement; // NOLINT } -namespace tesseract_scene_graph -{ -class SceneGraph; -} namespace tesseract_srdf { diff --git a/tesseract_srdf/include/tesseract_srdf/disabled_collisions.h b/tesseract_srdf/include/tesseract_srdf/disabled_collisions.h index 69375daa17f..35796c99bfc 100644 --- a/tesseract_srdf/include/tesseract_srdf/disabled_collisions.h +++ b/tesseract_srdf/include/tesseract_srdf/disabled_collisions.h @@ -32,18 +32,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT } -namespace tesseract_scene_graph -{ -class SceneGraph; -} -namespace tesseract_common -{ -class AllowedCollisionMatrix; -} // namespace tesseract_common namespace tesseract_srdf { diff --git a/tesseract_srdf/include/tesseract_srdf/fwd.h b/tesseract_srdf/include/tesseract_srdf/fwd.h new file mode 100644 index 00000000000..aed845c42cd --- /dev/null +++ b/tesseract_srdf/include/tesseract_srdf/fwd.h @@ -0,0 +1,35 @@ +/** + * @file fwd.h + * @brief Tesseract SRDF Forward Declarations + * + * @author Levi Armstrong + * @date February 20, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_SRDF_FWD_H +#define TESSERACT_SRDF_FWD_H + +namespace tesseract_srdf +{ +class SRDFModel; +struct KinematicsInformation; +} // namespace tesseract_srdf +#endif // TESSERACT_SRDF_FWD_H diff --git a/tesseract_srdf/include/tesseract_srdf/group_states.h b/tesseract_srdf/include/tesseract_srdf/group_states.h index 2f6300da5ed..4e23e8b371e 100644 --- a/tesseract_srdf/include/tesseract_srdf/group_states.h +++ b/tesseract_srdf/include/tesseract_srdf/group_states.h @@ -32,15 +32,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tinyxml2 { class XMLElement; // NOLINT } -namespace tesseract_scene_graph -{ -class SceneGraph; -} namespace tesseract_srdf { diff --git a/tesseract_srdf/include/tesseract_srdf/group_tool_center_points.h b/tesseract_srdf/include/tesseract_srdf/group_tool_center_points.h index 09959e9d234..790c59786b6 100644 --- a/tesseract_srdf/include/tesseract_srdf/group_tool_center_points.h +++ b/tesseract_srdf/include/tesseract_srdf/group_tool_center_points.h @@ -32,16 +32,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include namespace tinyxml2 { class XMLElement; // NOLINT } -namespace tesseract_scene_graph -{ -class SceneGraph; -} namespace tesseract_srdf { diff --git a/tesseract_srdf/include/tesseract_srdf/groups.h b/tesseract_srdf/include/tesseract_srdf/groups.h index b0605e1b341..bb58b7513ab 100644 --- a/tesseract_srdf/include/tesseract_srdf/groups.h +++ b/tesseract_srdf/include/tesseract_srdf/groups.h @@ -32,16 +32,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include namespace tinyxml2 { class XMLElement; // NOLINT } -namespace tesseract_scene_graph -{ -class SceneGraph; -} namespace tesseract_srdf { diff --git a/tesseract_srdf/include/tesseract_srdf/kinematics_information.h b/tesseract_srdf/include/tesseract_srdf/kinematics_information.h index a0505875c42..1e0f825d0dd 100644 --- a/tesseract_srdf/include/tesseract_srdf/kinematics_information.h +++ b/tesseract_srdf/include/tesseract_srdf/kinematics_information.h @@ -32,11 +32,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace tesseract_srdf { @@ -152,6 +153,5 @@ struct KinematicsInformation } // namespace tesseract_srdf #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_srdf::KinematicsInformation, "KinematicsInformation") #endif // TESSERACT_SRDF_KINEMATICS_INFORMATION_H diff --git a/tesseract_srdf/include/tesseract_srdf/srdf_model.h b/tesseract_srdf/include/tesseract_srdf/srdf_model.h index 4a1da5bdc72..10906e73e5c 100644 --- a/tesseract_srdf/include/tesseract_srdf/srdf_model.h +++ b/tesseract_srdf/include/tesseract_srdf/srdf_model.h @@ -36,18 +36,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include -#include #include -#include -#include +#include /// Main namespace namespace tesseract_srdf { -using CollisionMarginData = tesseract_common::CollisionMarginData; -using PairsCollisionMarginData = tesseract_common::PairsCollisionMarginData; - /** @brief Representation of semantic information about the robot */ class SRDFModel { @@ -104,7 +101,7 @@ class SRDFModel tesseract_common::AllowedCollisionMatrix acm; /** @brief Collision margin data */ - tesseract_common::CollisionMarginData::Ptr collision_margin_data; + std::shared_ptr collision_margin_data; /** @brief The calibration information */ tesseract_common::CalibrationInfo calibration_info; @@ -121,7 +118,6 @@ class SRDFModel } // namespace tesseract_srdf #include -#include BOOST_CLASS_EXPORT_KEY2(tesseract_srdf::SRDFModel, "SRDFModel") #endif // TESSERACT_SRDF_SRDF_MODEL_H diff --git a/tesseract_srdf/include/tesseract_srdf/utils.h b/tesseract_srdf/include/tesseract_srdf/utils.h index ec5e694739c..26f7621ad91 100644 --- a/tesseract_srdf/include/tesseract_srdf/utils.h +++ b/tesseract_srdf/include/tesseract_srdf/utils.h @@ -27,13 +27,15 @@ #define TESSERACT_SRDF_UTILS_H #include -#include + #include -#include -#include +#include +#include namespace tesseract_srdf { +class SRDFModel; + /** * @brief Add allowed collisions to the scene graph * @param scene_graph The scene graph to add allowed collisions data diff --git a/tesseract_srdf/src/collision_margins.cpp b/tesseract_srdf/src/collision_margins.cpp index b5d448a5524..1b1a5c8d3f4 100644 --- a/tesseract_srdf/src/collision_margins.cpp +++ b/tesseract_srdf/src/collision_margins.cpp @@ -49,8 +49,7 @@ tesseract_common::CollisionMarginData::Ptr parseCollisionMargins(const tesseract if (xml_element == nullptr) return nullptr; - tinyxml2::XMLError status = - tesseract_common::QueryDoubleAttributeRequired(xml_element, "default_margin", default_margin); + int status = tesseract_common::QueryDoubleAttributeRequired(xml_element, "default_margin", default_margin); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("CollisionMargins: collision_margins missing attribute " "'default_margin'.")); @@ -61,7 +60,7 @@ tesseract_common::CollisionMarginData::Ptr parseCollisionMargins(const tesseract { std::string link1_name, link2_name; double link_pair_margin{ 0 }; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(xml_pair_element, "link1", link1_name); + int status = tesseract_common::QueryStringAttributeRequired(xml_pair_element, "link1", link1_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("parseCollisionMargins: Missing or failed to parse 'link1' " "attribute.")); diff --git a/tesseract_srdf/src/configs.cpp b/tesseract_srdf/src/configs.cpp index 220f5ffea25..0e9cf44e489 100644 --- a/tesseract_srdf/src/configs.cpp +++ b/tesseract_srdf/src/configs.cpp @@ -26,11 +26,11 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include #include @@ -42,7 +42,7 @@ tesseract_common::fs::path parseConfigFilePath(const tesseract_common::ResourceL const std::array& /*version*/) { std::string filename; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(xml_element, "filename", filename); + int status = tesseract_common::QueryStringAttributeRequired(xml_element, "filename", filename); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error(std::string(xml_element->Value()) + ": Missing or failed to parse " "'filename' attribute.")); diff --git a/tesseract_srdf/src/disabled_collisions.cpp b/tesseract_srdf/src/disabled_collisions.cpp index 3b3715a866e..7ec672680c6 100644 --- a/tesseract_srdf/src/disabled_collisions.cpp +++ b/tesseract_srdf/src/disabled_collisions.cpp @@ -31,9 +31,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include -#include namespace tesseract_srdf { @@ -48,7 +48,7 @@ tesseract_common::AllowedCollisionMatrix parseDisabledCollisions(const tesseract xml_element = xml_element->NextSiblingElement("disable_collisions")) { std::string link1_name, link2_name, reason; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(xml_element, "link1", link1_name); + int status = tesseract_common::QueryStringAttributeRequired(xml_element, "link1", link1_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("DisabledCollisions: Missing or failed to parse attribute 'link1'!")); diff --git a/tesseract_srdf/src/group_states.cpp b/tesseract_srdf/src/group_states.cpp index b7699d12d8e..80c4a80b60e 100644 --- a/tesseract_srdf/src/group_states.cpp +++ b/tesseract_srdf/src/group_states.cpp @@ -46,7 +46,7 @@ GroupJointStates parseGroupStates(const tesseract_scene_graph::SceneGraph& scene xml_element = xml_element->NextSiblingElement("group_state")) { std::string group_name, state_name; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(xml_element, "group", group_name); + int status = tesseract_common::QueryStringAttributeRequired(xml_element, "group", group_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("GroupStates: Missing or failed to parse attribute 'group'!")); diff --git a/tesseract_srdf/src/group_tool_center_points.cpp b/tesseract_srdf/src/group_tool_center_points.cpp index f7960e5343e..db3b90296f6 100644 --- a/tesseract_srdf/src/group_tool_center_points.cpp +++ b/tesseract_srdf/src/group_tool_center_points.cpp @@ -56,8 +56,7 @@ GroupTCPs parseGroupTCPs(const tesseract_scene_graph::SceneGraph& /*scene_graph* xml_group_element = xml_group_element->NextSiblingElement("group_tcps")) { std::string group_name_string; - tinyxml2::XMLError status = - tesseract_common::QueryStringAttributeRequired(xml_group_element, "group", group_name_string); + int status = tesseract_common::QueryStringAttributeRequired(xml_group_element, "group", group_name_string); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("GroupTCPs: Missing or failed to parse attribute 'group'!")); @@ -72,7 +71,7 @@ GroupTCPs parseGroupTCPs(const tesseract_scene_graph::SceneGraph& /*scene_graph* strFormat("GroupTCPs: Invalid tcp definition for group '%s'!", group_name_string.c_str()))); std::string tcp_name_string; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(xml_element, "name", tcp_name_string); + int status = tesseract_common::QueryStringAttributeRequired(xml_element, "name", tcp_name_string); // LCOV_EXCL_START if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested( diff --git a/tesseract_srdf/src/groups.cpp b/tesseract_srdf/src/groups.cpp index 7ad96fac073..de2c6167938 100644 --- a/tesseract_srdf/src/groups.cpp +++ b/tesseract_srdf/src/groups.cpp @@ -52,7 +52,7 @@ parseGroups(const tesseract_scene_graph::SceneGraph& scene_graph, xml_element = xml_element->NextSiblingElement("group")) { std::string group_name; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(xml_element, "name", group_name); + int status = tesseract_common::QueryStringAttributeRequired(xml_element, "name", group_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Group: Missing or failed to parse attribute 'name'!")); @@ -65,7 +65,7 @@ parseGroups(const tesseract_scene_graph::SceneGraph& scene_graph, link_xml = link_xml->NextSiblingElement("link")) { std::string link_name; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(link_xml, "name", link_name); + int status = tesseract_common::QueryStringAttributeRequired(link_xml, "name", link_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error( strFormat("Group: '%s' link element is missing or failed to parse attribute 'name'!", group_name.c_str()))); @@ -82,7 +82,7 @@ parseGroups(const tesseract_scene_graph::SceneGraph& scene_graph, joint_xml = joint_xml->NextSiblingElement("joint")) { std::string joint_name; - tinyxml2::XMLError status = tesseract_common::QueryStringAttributeRequired(joint_xml, "name", joint_name); + int status = tesseract_common::QueryStringAttributeRequired(joint_xml, "name", joint_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' joint element is missing or failed to parse " "attribute 'name'!", @@ -100,8 +100,7 @@ parseGroups(const tesseract_scene_graph::SceneGraph& scene_graph, chain_xml = chain_xml->NextSiblingElement("chain")) { std::string base_link_name, tip_link_name; - tinyxml2::XMLError status = - tesseract_common::QueryStringAttributeRequired(chain_xml, "base_link", base_link_name); + int status = tesseract_common::QueryStringAttributeRequired(chain_xml, "base_link", base_link_name); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error(strFormat("Group: '%s' chain element is missing or failed to parse " "attribute " diff --git a/tesseract_srdf/src/kinematics_information.cpp b/tesseract_srdf/src/kinematics_information.cpp index 9e632cf3f0e..6c8fdcce20f 100644 --- a/tesseract_srdf/src/kinematics_information.cpp +++ b/tesseract_srdf/src/kinematics_information.cpp @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include diff --git a/tesseract_srdf/src/srdf_model.cpp b/tesseract_srdf/src/srdf_model.cpp index f9e07cd3e14..4b9a698569a 100644 --- a/tesseract_srdf/src/srdf_model.cpp +++ b/tesseract_srdf/src/srdf_model.cpp @@ -51,6 +51,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include namespace tesseract_srdf { @@ -91,8 +94,8 @@ void SRDFModel::initString(const tesseract_scene_graph::SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator) { tinyxml2::XMLDocument xml_doc; - tinyxml2::XMLError status = xml_doc.Parse(xmlstring.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) + int status = xml_doc.Parse(xmlstring.c_str()); + if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("SRDF: Failed to create XMLDocument from xml string!")); clear(); @@ -403,8 +406,8 @@ bool SRDFModel::saveToFile(const std::string& file_path) const } doc.InsertFirstChild(xml_root); - tinyxml2::XMLError status = doc.SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) + int status = doc.SaveFile(file_path.c_str()); + if (status != tinyxml2::XML_SUCCESS) { // LCOV_EXCL_START CONSOLE_BRIDGE_logError("Failed to save SRDF XML File: %s", file_path.c_str()); diff --git a/tesseract_srdf/src/utils.cpp b/tesseract_srdf/src/utils.cpp index cdbe69643f6..101ac35e332 100644 --- a/tesseract_srdf/src/utils.cpp +++ b/tesseract_srdf/src/utils.cpp @@ -24,6 +24,9 @@ * limitations under the License. */ #include +#include +#include +#include namespace tesseract_srdf { diff --git a/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp b/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp index 39f8b3ac5fa..fa3b7eff1ec 100644 --- a/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp +++ b/tesseract_srdf/test/tesseract_srdf_serialization_unit.cpp @@ -35,6 +35,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include using namespace tesseract_common; using namespace tesseract_scene_graph; diff --git a/tesseract_srdf/test/tesseract_srdf_unit.cpp b/tesseract_srdf/test/tesseract_srdf_unit.cpp index c9b5858e214..55468f86db9 100644 --- a/tesseract_srdf/test/tesseract_srdf_unit.cpp +++ b/tesseract_srdf/test/tesseract_srdf_unit.cpp @@ -4,10 +4,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -19,6 +21,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include enum class ABBConfig { diff --git a/tesseract_state_solver/include/tesseract_state_solver/fwd.h b/tesseract_state_solver/include/tesseract_state_solver/fwd.h new file mode 100644 index 00000000000..6fb238fb4b4 --- /dev/null +++ b/tesseract_state_solver/include/tesseract_state_solver/fwd.h @@ -0,0 +1,35 @@ +/** + * @file fwd.h + * @brief Tesseract Scene Graph State Solver Forward Declarations. + * + * @author Levi Armstrong + * @date Dec 18, 2017 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_STATE_SOLVER_FWD_H +#define TESSERACT_STATE_SOLVER_FWD_H + +namespace tesseract_scene_graph +{ +class StateSolver; +class MutableStateSolver; +} // namespace tesseract_scene_graph + +#endif // TESSERACT_STATE_SOLVER_FWD_H diff --git a/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h index 340264403e3..120f684cfa9 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/kdl/kdl_state_solver.h @@ -36,6 +36,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include namespace tesseract_scene_graph { diff --git a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_node.h b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_node.h index e4a7ae5cad1..1ee5f3bdcff 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_node.h +++ b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_node.h @@ -39,7 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_scene_graph { @@ -137,7 +137,7 @@ class OFKTNode * * @param static_tf The new static transformation */ - virtual void setStaticTransformation(Eigen::Isometry3d static_tf) = 0; + virtual void setStaticTransformation(const Eigen::Isometry3d& static_tf) = 0; /** * @brief Get the nodes static transformation diff --git a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h index 729b9d5aaed..1d807187f8c 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h +++ b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_nodes.h @@ -45,9 +45,9 @@ class OFKTBaseNode : public OFKTNode EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP - OFKTBaseNode(tesseract_scene_graph::JointType type, OFKTNode* parent, std::string link_name); + OFKTBaseNode(JointType type, OFKTNode* parent, std::string link_name); - OFKTBaseNode(tesseract_scene_graph::JointType type, + OFKTBaseNode(JointType type, OFKTNode* parent, std::string link_name, std::string joint_name, @@ -68,7 +68,7 @@ class OFKTBaseNode : public OFKTNode bool hasJointValueChanged() const override; - void setStaticTransformation(Eigen::Isometry3d static_tf) override; + void setStaticTransformation(const Eigen::Isometry3d& static_tf) override; const Eigen::Isometry3d& getStaticTransformation() const override; const Eigen::Isometry3d& getLocalTransformation() const override; @@ -125,7 +125,7 @@ class OFKTRootNode : public OFKTBaseNode void setParent(OFKTNode* parent) override; void storeJointValue(double joint_value) override; - void setStaticTransformation(Eigen::Isometry3d static_tf) override; + void setStaticTransformation(const Eigen::Isometry3d& static_tf) override; void computeAndStoreLocalTransformation() override; void computeAndStoreWorldTransformation() override; bool updateWorldTransformationRequired() const override; @@ -150,7 +150,7 @@ class OFKTFixedNode : public OFKTBaseNode void storeJointValue(double joint_value) override; double getJointValue() const override; - void setStaticTransformation(Eigen::Isometry3d static_tf) override; + void setStaticTransformation(const Eigen::Isometry3d& static_tf) override; void computeAndStoreLocalTransformation() override; Eigen::Isometry3d computeLocalTransformation(double joint_value) const override; diff --git a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h index b7a94fd056e..e5190d1a874 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/ofkt/ofkt_state_solver.h @@ -40,10 +40,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include +#include namespace tesseract_scene_graph { +class OFKTNode; + /** * @brief An implementation of the Optimized Forward Kinematic Tree as a stat solver * @@ -150,15 +153,15 @@ class OFKTStateSolver : public MutableStateSolver StateSolver::UPtr clone() const override final; private: - SceneState current_state_; /**< Current state of the scene */ - std::vector joint_names_; /**< The link names */ - std::vector active_joint_names_; /**< The active joint names */ - std::vector link_names_; /**< The link names */ - std::unordered_map nodes_; /**< The joint name map to node */ - std::unordered_map link_map_; /**< The link name map to node */ - tesseract_common::KinematicLimits limits_; /**< The kinematic limits */ - OFKTNode::UPtr root_; /**< The root node of the tree */ - int revision_{ 0 }; /**< The revision number */ + SceneState current_state_; /**< Current state of the scene */ + std::vector joint_names_; /**< The link names */ + std::vector active_joint_names_; /**< The active joint names */ + std::vector link_names_; /**< The link names */ + std::unordered_map> nodes_; /**< The joint name map to node */ + std::unordered_map link_map_; /**< The link name map to node */ + tesseract_common::KinematicLimits limits_; /**< The kinematic limits */ + std::unique_ptr root_; /**< The root node of the tree */ + int revision_{ 0 }; /**< The revision number */ /** @brief The state solver can be accessed from multiple threads, need use mutex throughout */ mutable std::shared_mutex mutex_; @@ -188,7 +191,8 @@ class OFKTStateSolver : public MutableStateSolver * @param parent_world_tf The nodes parent's world transformaiton * @param update_required Indicates if work transform update is required */ - void update(SceneState& state, const OFKTNode* node, Eigen::Isometry3d parent_world_tf, bool update_required) const; + void + update(SceneState& state, const OFKTNode* node, const Eigen::Isometry3d& parent_world_tf, bool update_required) const; /** * @brief Given a set of joint values calculate the jacobian for the provided link_name @@ -222,7 +226,7 @@ class OFKTStateSolver : public MutableStateSolver const std::string& joint_name, const std::string& parent_link_name, const std::string& child_link_name, - std::vector& new_joint_limits); + std::vector>& new_joint_limits); /** * @brief Remove a node and all of its children @@ -243,14 +247,14 @@ class OFKTStateSolver : public MutableStateSolver * @param new_kinematic_joints The vector to store new kinematic joints added to the solver * @param joint The joint performing the move */ - void moveLinkHelper(std::vector& new_joint_limits, const Joint& joint); + void moveLinkHelper(std::vector>& new_joint_limits, const Joint& joint); /** * @brief This is a helper function for replacing a joint * @param new_kinematic_joints The vector to store new kinematic joints added to the solver * @param joint The joint performing the replacement */ - void replaceJointHelper(std::vector& new_joint_limits, const Joint& joint); + void replaceJointHelper(std::vector>& new_joint_limits, const Joint& joint); /** * @brief This will clean up member variables joint_names_ and limits_ @@ -268,7 +272,7 @@ class OFKTStateSolver : public MutableStateSolver * @brief appends the new joint limits * @param new_joint_limits */ - void addNewJointLimits(const std::vector& new_joint_limits); + void addNewJointLimits(const std::vector>& new_joint_limits); friend struct ofkt_builder; }; diff --git a/tesseract_state_solver/include/tesseract_state_solver/state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/state_solver.h index 728aae2143e..fcce945bd42 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/state_solver.h @@ -36,9 +36,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include +#include namespace tesseract_scene_graph { diff --git a/tesseract_state_solver/src/kdl_state_solver.cpp b/tesseract_state_solver/src/kdl_state_solver.cpp index 95b1c23f570..a3c82c80a00 100644 --- a/tesseract_state_solver/src/kdl_state_solver.cpp +++ b/tesseract_state_solver/src/kdl_state_solver.cpp @@ -29,6 +29,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include +#include #include #include diff --git a/tesseract_state_solver/src/ofkt_nodes.cpp b/tesseract_state_solver/src/ofkt_nodes.cpp index 2abdda873df..d59f5820345 100644 --- a/tesseract_state_solver/src/ofkt_nodes.cpp +++ b/tesseract_state_solver/src/ofkt_nodes.cpp @@ -30,6 +30,7 @@ */ #include +#include #include namespace tesseract_scene_graph @@ -37,12 +38,12 @@ namespace tesseract_scene_graph /*********************************************************************/ /*************************** BASE NODE *******************************/ /*********************************************************************/ -OFKTBaseNode::OFKTBaseNode(tesseract_scene_graph::JointType type, OFKTNode* parent, std::string link_name) +OFKTBaseNode::OFKTBaseNode(JointType type, OFKTNode* parent, std::string link_name) : type_(type), parent_(parent), link_name_(std::move(link_name)) { } -OFKTBaseNode::OFKTBaseNode(tesseract_scene_graph::JointType type, +OFKTBaseNode::OFKTBaseNode(JointType type, OFKTNode* parent, std::string link_name, std::string joint_name, @@ -82,7 +83,7 @@ double OFKTBaseNode::getJointValue() const { return joint_value_; } bool OFKTBaseNode::hasJointValueChanged() const { return joint_value_changed_; } -void OFKTBaseNode::setStaticTransformation(Eigen::Isometry3d static_tf) +void OFKTBaseNode::setStaticTransformation(const Eigen::Isometry3d& static_tf) { static_tf_ = static_tf; local_tf_ = static_tf_ * joint_tf_; @@ -143,7 +144,7 @@ void OFKTRootNode::storeJointValue(double /*joint_value*/) throw std::runtime_error("OFKTRootNode: does not have a joint value!"); } -void OFKTRootNode::setStaticTransformation(Eigen::Isometry3d /*static_tf*/) +void OFKTRootNode::setStaticTransformation(const Eigen::Isometry3d& /*static_tf*/) { throw std::runtime_error("OFKTRootNode: does not have a static transform!"); } @@ -177,7 +178,7 @@ void OFKTFixedNode::storeJointValue(double /*joint_value*/) double OFKTFixedNode::getJointValue() const { throw std::runtime_error("OFKTFixedNode: does not have a joint value!"); } -void OFKTFixedNode::setStaticTransformation(Eigen::Isometry3d static_tf) +void OFKTFixedNode::setStaticTransformation(const Eigen::Isometry3d& static_tf) { static_tf_ = static_tf; local_tf_ = static_tf; diff --git a/tesseract_state_solver/src/ofkt_state_solver.cpp b/tesseract_state_solver/src/ofkt_state_solver.cpp index 8a7ba5a4a58..29969561c24 100644 --- a/tesseract_state_solver/src/ofkt_state_solver.cpp +++ b/tesseract_state_solver/src/ofkt_state_solver.cpp @@ -32,11 +32,15 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include +#include +#include +#include #include namespace tesseract_scene_graph @@ -805,35 +809,36 @@ void OFKTStateSolver::update(OFKTNode* node, bool update_required) void OFKTStateSolver::update(SceneState& state, const OFKTNode* node, - Eigen::Isometry3d parent_world_tf, + const Eigen::Isometry3d& parent_world_tf, bool update_required) const { + Eigen::Isometry3d updated_parent_world_tf; if (node->getType() != tesseract_scene_graph::JointType::FIXED) { double jv = state.joints[node->getJointName()]; if (!tesseract_common::almostEqualRelativeAndAbs(node->getJointValue(), jv, 1e-8)) { - parent_world_tf = parent_world_tf * node->computeLocalTransformation(jv); + updated_parent_world_tf = parent_world_tf * node->computeLocalTransformation(jv); update_required = true; } else { - parent_world_tf = parent_world_tf * node->getLocalTransformation(); + updated_parent_world_tf = parent_world_tf * node->getLocalTransformation(); } } else { - parent_world_tf = parent_world_tf * node->getLocalTransformation(); + updated_parent_world_tf = parent_world_tf * node->getLocalTransformation(); } if (update_required) { - state.link_transforms[node->getLinkName()] = parent_world_tf; - state.joint_transforms[node->getJointName()] = parent_world_tf; + state.link_transforms[node->getLinkName()] = updated_parent_world_tf; + state.joint_transforms[node->getJointName()] = updated_parent_world_tf; } for (const auto* child : node->getChildren()) - update(state, child, parent_world_tf, update_required); + update(state, child, updated_parent_world_tf, update_required); } bool OFKTStateSolver::initHelper(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& prefix) @@ -878,7 +883,8 @@ bool OFKTStateSolver::initHelper(const tesseract_scene_graph::SceneGraph& scene_ return true; } -void OFKTStateSolver::moveLinkHelper(std::vector& new_joint_limits, const Joint& joint) +void OFKTStateSolver::moveLinkHelper(std::vector>& new_joint_limits, + const Joint& joint) { auto* old_node = link_map_[joint.child_link_name]; const std::string old_joint_name = old_node->getJointName(); @@ -921,7 +927,8 @@ void OFKTStateSolver::moveLinkHelper(std::vector& new_joi update(replaced_node.get(), true); } -void OFKTStateSolver::replaceJointHelper(std::vector& new_joint_limits, const Joint& joint) +void OFKTStateSolver::replaceJointHelper(std::vector>& new_joint_limits, + const Joint& joint) { auto& n = nodes_[joint.getName()]; @@ -1003,7 +1010,7 @@ void OFKTStateSolver::addNode(const tesseract_scene_graph::Joint& joint, const std::string& joint_name, const std::string& parent_link_name, const std::string& child_link_name, - std::vector& new_joint_limits) + std::vector>& new_joint_limits) { switch (joint.type) { @@ -1116,7 +1123,7 @@ void OFKTStateSolver::removeNode(OFKTNode* node, nodes_.erase(node->getJointName()); } -void OFKTStateSolver::addNewJointLimits(const std::vector& new_joint_limits) +void OFKTStateSolver::addNewJointLimits(const std::vector>& new_joint_limits) { // Populate Joint Limits if (!new_joint_limits.empty()) diff --git a/tesseract_state_solver/test/state_solver_test_suite.h b/tesseract_state_solver/test/state_solver_test_suite.h index 19ff271f2d3..fc6cc1a3cf9 100644 --- a/tesseract_state_solver/test/state_solver_test_suite.h +++ b/tesseract_state_solver/test/state_solver_test_suite.h @@ -12,6 +12,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include #include #include diff --git a/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h b/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h index 045c5673e62..761ba1e8897 100644 --- a/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h +++ b/tesseract_support/include/tesseract_support/tesseract_support_resource_locator.h @@ -33,7 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include namespace tesseract_common { diff --git a/tesseract_support/src/tesseract_support_resource_locator.cpp b/tesseract_support/src/tesseract_support_resource_locator.cpp index bd8e195dd67..d4434ab3809 100644 --- a/tesseract_support/src/tesseract_support_resource_locator.cpp +++ b/tesseract_support/src/tesseract_support_resource_locator.cpp @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_common { diff --git a/tesseract_urdf/include/tesseract_urdf/box.h b/tesseract_urdf/include/tesseract_urdf/box.h index c8ce85f7893..26df475c380 100644 --- a/tesseract_urdf/include/tesseract_urdf/box.h +++ b/tesseract_urdf/include/tesseract_urdf/box.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_geometry -{ -class Box; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/calibration.h b/tesseract_urdf/include/tesseract_urdf/calibration.h index d88b5e1c96e..61bba134a75 100644 --- a/tesseract_urdf/include/tesseract_urdf/calibration.h +++ b/tesseract_urdf/include/tesseract_urdf/calibration.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class JointCalibration; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/capsule.h b/tesseract_urdf/include/tesseract_urdf/capsule.h index 367d869a0d5..076294ab7ae 100644 --- a/tesseract_urdf/include/tesseract_urdf/capsule.h +++ b/tesseract_urdf/include/tesseract_urdf/capsule.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_geometry -{ -class Capsule; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/collision.h b/tesseract_urdf/include/tesseract_urdf/collision.h index 2f7f6e6b06b..6d5a06431c2 100644 --- a/tesseract_urdf/include/tesseract_urdf/collision.h +++ b/tesseract_urdf/include/tesseract_urdf/collision.h @@ -32,19 +32,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class Collision; -} -namespace tesseract_common -{ -class ResourceLocator; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/cone.h b/tesseract_urdf/include/tesseract_urdf/cone.h index 0f19abb27e7..63b826560f9 100644 --- a/tesseract_urdf/include/tesseract_urdf/cone.h +++ b/tesseract_urdf/include/tesseract_urdf/cone.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_geometry -{ -class Cone; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h b/tesseract_urdf/include/tesseract_urdf/convex_mesh.h index ed1ace95f5a..64d10dc69ef 100644 --- a/tesseract_urdf/include/tesseract_urdf/convex_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/convex_mesh.h @@ -32,22 +32,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - -namespace tesseract_geometry -{ -class ConvexMesh; -} - namespace tesseract_urdf { /** diff --git a/tesseract_urdf/include/tesseract_urdf/cylinder.h b/tesseract_urdf/include/tesseract_urdf/cylinder.h index 3c3117cc88f..6053a5768ca 100644 --- a/tesseract_urdf/include/tesseract_urdf/cylinder.h +++ b/tesseract_urdf/include/tesseract_urdf/cylinder.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_geometry -{ -class Cylinder; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/dynamics.h b/tesseract_urdf/include/tesseract_urdf/dynamics.h index bb79728b21e..090457552c2 100644 --- a/tesseract_urdf/include/tesseract_urdf/dynamics.h +++ b/tesseract_urdf/include/tesseract_urdf/dynamics.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class JointDynamics; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/geometry.h b/tesseract_urdf/include/tesseract_urdf/geometry.h index dbac4d328fc..daf91b7584e 100644 --- a/tesseract_urdf/include/tesseract_urdf/geometry.h +++ b/tesseract_urdf/include/tesseract_urdf/geometry.h @@ -32,22 +32,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - -namespace tesseract_geometry -{ -class Geometry; -} - namespace tesseract_urdf { /** diff --git a/tesseract_urdf/include/tesseract_urdf/inertial.h b/tesseract_urdf/include/tesseract_urdf/inertial.h index 3e509ba691f..825dda7cf4d 100644 --- a/tesseract_urdf/include/tesseract_urdf/inertial.h +++ b/tesseract_urdf/include/tesseract_urdf/inertial.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class Inertial; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/joint.h b/tesseract_urdf/include/tesseract_urdf/joint.h index 2a9738b41cd..0cba9d9995f 100644 --- a/tesseract_urdf/include/tesseract_urdf/joint.h +++ b/tesseract_urdf/include/tesseract_urdf/joint.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class Joint; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/limits.h b/tesseract_urdf/include/tesseract_urdf/limits.h index cb1ebbae28b..625400afc6a 100644 --- a/tesseract_urdf/include/tesseract_urdf/limits.h +++ b/tesseract_urdf/include/tesseract_urdf/limits.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class JointLimits; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/link.h b/tesseract_urdf/include/tesseract_urdf/link.h index f5ead9b40ef..353ee4e6347 100644 --- a/tesseract_urdf/include/tesseract_urdf/link.h +++ b/tesseract_urdf/include/tesseract_urdf/link.h @@ -32,21 +32,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class Link; -class Material; -} // namespace tesseract_scene_graph - -namespace tesseract_common -{ -class ResourceLocator; -} // namespace tesseract_common namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/material.h b/tesseract_urdf/include/tesseract_urdf/material.h index 47f36cb37e8..9071ecd9530 100644 --- a/tesseract_urdf/include/tesseract_urdf/material.h +++ b/tesseract_urdf/include/tesseract_urdf/material.h @@ -32,15 +32,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class Material; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/mesh.h b/tesseract_urdf/include/tesseract_urdf/mesh.h index 67486662743..bcbe8aab8b3 100644 --- a/tesseract_urdf/include/tesseract_urdf/mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/mesh.h @@ -32,22 +32,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - -namespace tesseract_geometry -{ -class Mesh; -} - namespace tesseract_urdf { /** diff --git a/tesseract_urdf/include/tesseract_urdf/mimic.h b/tesseract_urdf/include/tesseract_urdf/mimic.h index ce56f3477c0..fc11c4a3f57 100644 --- a/tesseract_urdf/include/tesseract_urdf/mimic.h +++ b/tesseract_urdf/include/tesseract_urdf/mimic.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class JointMimic; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/octomap.h b/tesseract_urdf/include/tesseract_urdf/octomap.h index 2169d101a66..5bfc7586487 100644 --- a/tesseract_urdf/include/tesseract_urdf/octomap.h +++ b/tesseract_urdf/include/tesseract_urdf/octomap.h @@ -31,22 +31,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - -namespace tesseract_geometry -{ -class Octree; -} - namespace tesseract_urdf { /** diff --git a/tesseract_urdf/include/tesseract_urdf/octree.h b/tesseract_urdf/include/tesseract_urdf/octree.h index 8165a56cad7..323e87aec92 100644 --- a/tesseract_urdf/include/tesseract_urdf/octree.h +++ b/tesseract_urdf/include/tesseract_urdf/octree.h @@ -31,7 +31,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include +#include namespace tinyxml2 { @@ -39,11 +40,6 @@ class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - namespace tesseract_urdf { /** @@ -56,7 +52,7 @@ namespace tesseract_urdf */ std::shared_ptr parseOctree(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - tesseract_geometry::Octree::SubType shape_type, + tesseract_geometry::OctreeSubType shape_type, bool prune, int version); diff --git a/tesseract_urdf/include/tesseract_urdf/point_cloud.h b/tesseract_urdf/include/tesseract_urdf/point_cloud.h index f4c983f2b71..07a7a710a72 100644 --- a/tesseract_urdf/include/tesseract_urdf/point_cloud.h +++ b/tesseract_urdf/include/tesseract_urdf/point_cloud.h @@ -31,7 +31,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include +#include namespace tinyxml2 { @@ -39,11 +40,6 @@ class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - namespace tesseract_urdf { /** @@ -57,7 +53,7 @@ namespace tesseract_urdf */ std::shared_ptr parsePointCloud(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - tesseract_geometry::Octree::SubType shape_type, + tesseract_geometry::OctreeSubType shape_type, bool prune, int version); } // namespace tesseract_urdf diff --git a/tesseract_urdf/include/tesseract_urdf/safety_controller.h b/tesseract_urdf/include/tesseract_urdf/safety_controller.h index d144bc3cbb7..c5fe2745d1a 100644 --- a/tesseract_urdf/include/tesseract_urdf/safety_controller.h +++ b/tesseract_urdf/include/tesseract_urdf/safety_controller.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class JointSafety; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h index 5cbd47ea3ac..5111bd43e81 100644 --- a/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h +++ b/tesseract_urdf/include/tesseract_urdf/sdf_mesh.h @@ -32,22 +32,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_common -{ -class ResourceLocator; -} - -namespace tesseract_geometry -{ -class SDFMesh; -} - namespace tesseract_urdf { /** diff --git a/tesseract_urdf/include/tesseract_urdf/sphere.h b/tesseract_urdf/include/tesseract_urdf/sphere.h index 9806e575d98..46f6c7d0e08 100644 --- a/tesseract_urdf/include/tesseract_urdf/sphere.h +++ b/tesseract_urdf/include/tesseract_urdf/sphere.h @@ -31,15 +31,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_geometry -{ -class Sphere; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/urdf_parser.h b/tesseract_urdf/include/tesseract_urdf/urdf_parser.h index aa88dd8b783..fedf07ebf3d 100644 --- a/tesseract_urdf/include/tesseract_urdf/urdf_parser.h +++ b/tesseract_urdf/include/tesseract_urdf/urdf_parser.h @@ -26,8 +26,14 @@ #ifndef TESSERACT_URDF_URDF_PARSER_H #define TESSERACT_URDF_URDF_PARSER_H -#include -#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include namespace tesseract_urdf { @@ -39,8 +45,8 @@ namespace tesseract_urdf * the nested exception. * @return Tesseract Scene Graph, nullptr if failed to parse URDF */ -tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_xml_string, - const tesseract_common::ResourceLocator& locator); +std::unique_ptr parseURDFString(const std::string& urdf_xml_string, + const tesseract_common::ResourceLocator& locator); /** * @brief Parse a URDF file into a Tesseract Scene Graph @@ -50,10 +56,10 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_ * the nested exception. * @return Tesseract Scene Graph, nullptr if failed to parse URDF */ -tesseract_scene_graph::SceneGraph::UPtr parseURDFFile(const std::string& path, - const tesseract_common::ResourceLocator& locator); +std::unique_ptr parseURDFFile(const std::string& path, + const tesseract_common::ResourceLocator& locator); -void writeURDFFile(const tesseract_scene_graph::SceneGraph::ConstPtr& sg, +void writeURDFFile(const std::shared_ptr& sg, const std::string& package_path, const std::string& urdf_name = ""); } // namespace tesseract_urdf diff --git a/tesseract_urdf/include/tesseract_urdf/utils.h b/tesseract_urdf/include/tesseract_urdf/utils.h index 98f36b0c80d..51ef55ea133 100644 --- a/tesseract_urdf/include/tesseract_urdf/utils.h +++ b/tesseract_urdf/include/tesseract_urdf/utils.h @@ -4,11 +4,11 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include - +#include // #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_urdf { diff --git a/tesseract_urdf/include/tesseract_urdf/visual.h b/tesseract_urdf/include/tesseract_urdf/visual.h index eb2087dcf0a..bca2a188a41 100644 --- a/tesseract_urdf/include/tesseract_urdf/visual.h +++ b/tesseract_urdf/include/tesseract_urdf/visual.h @@ -33,20 +33,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + namespace tinyxml2 { class XMLElement; // NOLINT class XMLDocument; } // namespace tinyxml2 -namespace tesseract_scene_graph -{ -class Visual; -class Material; -} // namespace tesseract_scene_graph -namespace tesseract_common -{ -class ResourceLocator; -} namespace tesseract_urdf { diff --git a/tesseract_urdf/src/collision.cpp b/tesseract_urdf/src/collision.cpp index 36061bf4558..7232b8ac95c 100644 --- a/tesseract_urdf/src/collision.cpp +++ b/tesseract_urdf/src/collision.cpp @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include diff --git a/tesseract_urdf/src/dynamics.cpp b/tesseract_urdf/src/dynamics.cpp index 9803b38c049..4f419855c7e 100644 --- a/tesseract_urdf/src/dynamics.cpp +++ b/tesseract_urdf/src/dynamics.cpp @@ -45,7 +45,7 @@ tesseract_scene_graph::JointDynamics::Ptr tesseract_urdf::parseDynamics(const ti auto dynamics = std::make_shared(); - tinyxml2::XMLError status = xml_element->QueryDoubleAttribute("damping", &(dynamics->damping)); + int status = xml_element->QueryDoubleAttribute("damping", &(dynamics->damping)); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Dynamics: Error parsing attribute 'damping'!")); diff --git a/tesseract_urdf/src/geometry.cpp b/tesseract_urdf/src/geometry.cpp index ae4c847a015..ad49fafb4f6 100644 --- a/tesseract_urdf/src/geometry.cpp +++ b/tesseract_urdf/src/geometry.cpp @@ -60,7 +60,7 @@ tesseract_urdf::parseGeometry(const tinyxml2::XMLElement* xml_element, std::throw_with_nested(std::runtime_error("Geometry: Error missing 'geometry' element!")); std::string geometry_type; - tinyxml2::XMLError status = tesseract_common::QueryStringValue(geometry, geometry_type); + int status = tesseract_common::QueryStringValue(geometry, geometry_type); if (status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Geometry: Error parsing 'geometry' element, invalid geometry type!")); diff --git a/tesseract_urdf/src/limits.cpp b/tesseract_urdf/src/limits.cpp index 1b19154a21f..9764c6b995d 100644 --- a/tesseract_urdf/src/limits.cpp +++ b/tesseract_urdf/src/limits.cpp @@ -41,7 +41,7 @@ tesseract_scene_graph::JointLimits::Ptr tesseract_urdf::parseLimits(const tinyxm { auto limits = std::make_shared(); - tinyxml2::XMLError status = xml_element->QueryDoubleAttribute("lower", &(limits->lower)); + int status = xml_element->QueryDoubleAttribute("lower", &(limits->lower)); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Limits: Missing or failed to parse attribute 'lower'!")); diff --git a/tesseract_urdf/src/mimic.cpp b/tesseract_urdf/src/mimic.cpp index 75dfe530504..b36bec0a71d 100644 --- a/tesseract_urdf/src/mimic.cpp +++ b/tesseract_urdf/src/mimic.cpp @@ -51,7 +51,7 @@ tesseract_scene_graph::JointMimic::Ptr tesseract_urdf::parseMimic(const tinyxml2 else if (xml_element->Attribute("offset") == nullptr && xml_element->Attribute("multiplier") != nullptr) CONSOLE_BRIDGE_logDebug("Mimic: Missing attribute 'offset', using default value 1!"); - tinyxml2::XMLError s = xml_element->QueryDoubleAttribute("offset", &(m->offset)); + int s = xml_element->QueryDoubleAttribute("offset", &(m->offset)); if (s != tinyxml2::XML_NO_ATTRIBUTE && s != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Mimic: Error parsing attribute 'offset'!")); diff --git a/tesseract_urdf/src/octomap.cpp b/tesseract_urdf/src/octomap.cpp index d35f7da090b..44e2fe9c087 100644 --- a/tesseract_urdf/src/octomap.cpp +++ b/tesseract_urdf/src/octomap.cpp @@ -51,13 +51,13 @@ tesseract_geometry::Octree::Ptr tesseract_urdf::parseOctomap(const tinyxml2::XML if (tesseract_common::QueryStringAttribute(xml_element, "shape_type", shape_type) != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Octomap: Missing or failed parsing attribute 'shape_type'!")); - tesseract_geometry::Octree::SubType sub_type{ tesseract_geometry::Octree::SubType::BOX }; + tesseract_geometry::OctreeSubType sub_type{ tesseract_geometry::OctreeSubType::BOX }; if (shape_type == "box") - sub_type = tesseract_geometry::Octree::SubType::BOX; + sub_type = tesseract_geometry::OctreeSubType::BOX; else if (shape_type == "sphere_inside") - sub_type = tesseract_geometry::Octree::SubType::SPHERE_INSIDE; + sub_type = tesseract_geometry::OctreeSubType::SPHERE_INSIDE; else if (shape_type == "sphere_outside") - sub_type = tesseract_geometry::Octree::SubType::SPHERE_OUTSIDE; + sub_type = tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE; else std::throw_with_nested(std::runtime_error("Octomap: Invalid sub shape type, must be 'box', 'sphere_inside', or " "'sphere_outside'!")); @@ -106,11 +106,11 @@ tinyxml2::XMLElement* tesseract_urdf::writeOctomap(const std::shared_ptrgetSubType() == tesseract_geometry::Octree::SubType::BOX) + if (octree->getSubType() == tesseract_geometry::OctreeSubType::BOX) type_string = "box"; - else if (octree->getSubType() == tesseract_geometry::Octree::SubType::SPHERE_INSIDE) + else if (octree->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_INSIDE) type_string = "sphere_inside"; - else if (octree->getSubType() == tesseract_geometry::Octree::SubType::SPHERE_OUTSIDE) + else if (octree->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE) type_string = "sphere_outside"; else std::throw_with_nested(std::runtime_error("Octree subtype is invalid and cannot be converted to XML")); diff --git a/tesseract_urdf/src/octree.cpp b/tesseract_urdf/src/octree.cpp index cc1ca0c6d46..229f74d0f9f 100644 --- a/tesseract_urdf/src/octree.cpp +++ b/tesseract_urdf/src/octree.cpp @@ -32,15 +32,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include tesseract_geometry::Octree::Ptr tesseract_urdf::parseOctree(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - tesseract_geometry::Octree::SubType shape_type, + tesseract_geometry::OctreeSubType shape_type, bool prune, int /*version*/) { diff --git a/tesseract_urdf/src/origin.cpp b/tesseract_urdf/src/origin.cpp index b129169cad7..88c13c24377 100644 --- a/tesseract_urdf/src/origin.cpp +++ b/tesseract_urdf/src/origin.cpp @@ -47,7 +47,7 @@ Eigen::Isometry3d tesseract_urdf::parseOrigin(const tinyxml2::XMLElement* xml_el "for origin element!")); std::string xyz_string, rpy_string, wxyz_string; - tinyxml2::XMLError status = tesseract_common::QueryStringAttribute(xml_element, "xyz", xyz_string); + int status = tesseract_common::QueryStringAttribute(xml_element, "xyz", xyz_string); if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'xyz'!")); diff --git a/tesseract_urdf/src/point_cloud.cpp b/tesseract_urdf/src/point_cloud.cpp index 5c90c1206f1..25597b82f58 100644 --- a/tesseract_urdf/src/point_cloud.cpp +++ b/tesseract_urdf/src/point_cloud.cpp @@ -32,14 +32,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include tesseract_geometry::Octree::Ptr tesseract_urdf::parsePointCloud(const tinyxml2::XMLElement* xml_element, const tesseract_common::ResourceLocator& locator, - tesseract_geometry::Octree::SubType shape_type, + tesseract_geometry::OctreeSubType shape_type, bool prune, int /*version*/) { diff --git a/tesseract_urdf/src/urdf_parser.cpp b/tesseract_urdf/src/urdf_parser.cpp index 2d924f4da08..587e0a51cd9 100644 --- a/tesseract_urdf/src/urdf_parser.cpp +++ b/tesseract_urdf/src/urdf_parser.cpp @@ -34,6 +34,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include +#include + #include #include #include @@ -42,8 +47,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_urdf { -tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_xml_string, - const tesseract_common::ResourceLocator& locator) +std::unique_ptr parseURDFString(const std::string& urdf_xml_string, + const tesseract_common::ResourceLocator& locator) { tinyxml2::XMLDocument xml_doc; if (xml_doc.Parse(urdf_xml_string.c_str()) != tinyxml2::XML_SUCCESS) @@ -169,8 +174,8 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_ return sg; } -tesseract_scene_graph::SceneGraph::UPtr parseURDFFile(const std::string& path, - const tesseract_common::ResourceLocator& locator) +std::unique_ptr parseURDFFile(const std::string& path, + const tesseract_common::ResourceLocator& locator) { std::ifstream ifs(path); if (!ifs) @@ -190,7 +195,7 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFFile(const std::string& path, return sg; } -void writeURDFFile(const tesseract_scene_graph::SceneGraph::ConstPtr& sg, +void writeURDFFile(const std::shared_ptr& sg, const std::string& package_path, const std::string& urdf_name) { diff --git a/tesseract_urdf/src/utils.cpp b/tesseract_urdf/src/utils.cpp index 02672e2f6e7..449282234ae 100644 --- a/tesseract_urdf/src/utils.cpp +++ b/tesseract_urdf/src/utils.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include namespace tesseract_urdf diff --git a/tesseract_urdf/src/visual.cpp b/tesseract_urdf/src/visual.cpp index 25e6c128da2..85a27b4c8ee 100644 --- a/tesseract_urdf/src/visual.cpp +++ b/tesseract_urdf/src/visual.cpp @@ -38,6 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include std::vector diff --git a/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp b/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp index 03748652f6c..4f0f7dd9658 100644 --- a/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_calibration_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_common_unit.h b/tesseract_urdf/test/tesseract_urdf_common_unit.h index b5ed6114e79..239f8d56826 100644 --- a/tesseract_urdf/test/tesseract_urdf_common_unit.h +++ b/tesseract_urdf/test/tesseract_urdf_common_unit.h @@ -12,6 +12,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include diff --git a/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp b/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp index 897fcaa7748..d3ee8c2b309 100644 --- a/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_dynamics_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp index 2cbf8fb91fc..07d9c391b6c 100644 --- a/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_geometry_unit.cpp @@ -2,6 +2,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -347,7 +348,7 @@ TEST(TesseractURDFUnit, write_geometry) // NOLINT { // octree tesseract_geometry::Geometry::Ptr geometry = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::BOX); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::BOX); std::string text; EXPECT_EQ( 0, @@ -358,7 +359,7 @@ TEST(TesseractURDFUnit, write_geometry) // NOLINT { // octree failed-to-write tesseract_geometry::Geometry::Ptr geometry = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::BOX); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::BOX); std::string text; EXPECT_EQ(1, writeTest( diff --git a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp index 0e297c33649..82fdb39447a 100644 --- a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp b/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp index f5e598623e5..099141cfb37 100644 --- a/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_limits_unit.cpp @@ -6,6 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp index 0327c0edf7a..00a331076de 100644 --- a/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mesh_material_unit.cpp @@ -6,6 +6,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp b/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp index 4e3e2c27294..7410b96def9 100644 --- a/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_mimic_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp index 5111f1720eb..3aabb16ba86 100644 --- a/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_octree_unit.cpp @@ -3,8 +3,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include @@ -36,7 +38,7 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); - EXPECT_TRUE(geom->getSubType() == geom->BOX); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } @@ -48,7 +50,7 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); - EXPECT_TRUE(geom->getSubType() == geom->BOX); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } @@ -60,7 +62,7 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); - EXPECT_TRUE(geom->getSubType() == geom->SPHERE_INSIDE); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_INSIDE); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } @@ -73,7 +75,7 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); - EXPECT_TRUE(geom->getSubType() == geom->BOX); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 1000); EXPECT_NEAR(geom->getOctree()->getResolution(), 0.1, 1e-5); @@ -86,7 +88,7 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); - EXPECT_TRUE(geom->getSubType() == geom->BOX); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::BOX); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 496); EXPECT_NEAR(geom->getOctree()->getResolution(), 0.1, 1e-5); @@ -109,7 +111,7 @@ TEST(TesseractURDFUnit, parse_octree) // NOLINT tesseract_geometry::Octree::Ptr geom; EXPECT_TRUE(runTest( geom, &tesseract_urdf::parseOctomap, str, "octomap", resource_locator, 2, true)); - EXPECT_TRUE(geom->getSubType() == geom->SPHERE_OUTSIDE); + EXPECT_TRUE(geom->getSubType() == tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE); EXPECT_TRUE(geom->getOctree() != nullptr); EXPECT_EQ(geom->calcNumSubShapes(), 8); } @@ -171,7 +173,7 @@ TEST(TesseractURDFUnit, write_octree) // NOLINT { { tesseract_geometry::Octree::Ptr geom = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::BOX); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::BOX); std::string text; EXPECT_EQ(0, writeTest( @@ -181,7 +183,7 @@ TEST(TesseractURDFUnit, write_octree) // NOLINT { // Trigger failed-to-write tesseract_geometry::Octree::Ptr geom = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::BOX); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::BOX); std::string text; EXPECT_EQ(1, writeTest( @@ -203,7 +205,7 @@ TEST(TesseractURDFUnit, write_octomap) // NOLINT { { // box tesseract_geometry::Octree::Ptr geom = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::BOX); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::BOX); std::string text; EXPECT_EQ(0, writeTest( @@ -213,7 +215,7 @@ TEST(TesseractURDFUnit, write_octomap) // NOLINT { // sphere inside tesseract_geometry::Octree::Ptr geom = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::SPHERE_INSIDE); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::SPHERE_INSIDE); std::string text; EXPECT_EQ(0, writeTest( @@ -223,7 +225,7 @@ TEST(TesseractURDFUnit, write_octomap) // NOLINT { // sphere outside tesseract_geometry::Octree::Ptr geom = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::SPHERE_OUTSIDE); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE); std::string text; EXPECT_EQ(0, writeTest( @@ -233,7 +235,7 @@ TEST(TesseractURDFUnit, write_octomap) // NOLINT { // Trigger failed-to-write tesseract_geometry::Octree::Ptr geom = std::make_shared( - std::make_shared(1.0), tesseract_geometry::Octree::SubType::BOX); + std::make_shared(1.0), tesseract_geometry::OctreeSubType::BOX); std::string text; EXPECT_EQ(1, writeTest( diff --git a/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp b/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp index 11de23e09cc..5696f19ab69 100644 --- a/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_safety_controller_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp index d9086f10980..915a5fdce9a 100644 --- a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp @@ -5,6 +5,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include #include #include "tesseract_urdf_common_unit.h" diff --git a/tesseract_visualization/include/tesseract_visualization/trajectory_player.h b/tesseract_visualization/include/tesseract_visualization/trajectory_player.h index 92c1ee52dd0..3e5f91cf5ae 100644 --- a/tesseract_visualization/include/tesseract_visualization/trajectory_player.h +++ b/tesseract_visualization/include/tesseract_visualization/trajectory_player.h @@ -29,12 +29,15 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include +#include namespace tesseract_visualization { +class TrajectoryInterpolator; + /** @brief Enables the ability to play a trajectory provided by the set program */ class TrajectoryPlayer { @@ -123,7 +126,7 @@ class TrajectoryPlayer long size() const; private: - TrajectoryInterpolator::UPtr trajectory_{ nullptr }; + std::unique_ptr trajectory_{ nullptr }; double trajectory_duration_start_{ 0 }; double trajectory_duration_end_{ 0 }; double current_duration_{ 0 }; diff --git a/tesseract_visualization/include/tesseract_visualization/visualization_loader.h b/tesseract_visualization/include/tesseract_visualization/visualization_loader.h index 0335620422e..5797542bcae 100644 --- a/tesseract_visualization/include/tesseract_visualization/visualization_loader.h +++ b/tesseract_visualization/include/tesseract_visualization/visualization_loader.h @@ -26,11 +26,17 @@ #ifndef TESSERACT_VISUALIZATION_VISUALIZATION_LOADER_H #define TESSERACT_VISUALIZATION_VISUALIZATION_LOADER_H -#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + #include namespace tesseract_visualization { +class Visualization; + /** * @brief This is used to dynamically load tesseract visualizer. * This class must remain around for the life of the loaded visualization class. @@ -46,7 +52,7 @@ class VisualizationLoader : public tesseract_common::PluginLoader * @param plugin_name The plugin name, if empty it tries to load default plugin * @return Returns nullptr if failed */ - Visualization::Ptr get(std::string plugin_name = "") const; + std::shared_ptr get(std::string plugin_name = "") const; }; } // namespace tesseract_visualization diff --git a/tesseract_visualization/src/trajectory_player.cpp b/tesseract_visualization/src/trajectory_player.cpp index caf9f0c4330..9f1a072f2e8 100644 --- a/tesseract_visualization/src/trajectory_player.cpp +++ b/tesseract_visualization/src/trajectory_player.cpp @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_visualization { diff --git a/tesseract_visualization/src/visualization_loader.cpp b/tesseract_visualization/src/visualization_loader.cpp index af55f123379..84ad633c7e7 100644 --- a/tesseract_visualization/src/visualization_loader.cpp +++ b/tesseract_visualization/src/visualization_loader.cpp @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include const std::string TESSERACT_IGNITION_LIBRARY_NAME = "tesseract_visualization_ignition_visualization_plugin"; @@ -48,7 +49,7 @@ VisualizationLoader::VisualizationLoader() search_paths.insert(TESSERACT_VISUALIZATION_PLUGIN_PATH); } -Visualization::Ptr VisualizationLoader::get(std::string plugin_name) const +std::shared_ptr VisualizationLoader::get(std::string plugin_name) const { if (plugin_name.empty()) plugin_name = TESSERACT_IGNITION_SYMBOL_NAME;