From f9665d4cb49f3ae962b812ca617fab5bd9350d4a Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 19 Feb 2024 22:36:08 -0600 Subject: [PATCH] Leverage forward declarations to improve compile times --- .../bullet/bullet_factories.h | 12 +- .../bullet/src/bullet_factories.cpp | 19 +- .../bullet/src/bullet_utils.cpp | 6 +- .../core/contact_managers_plugin_factory.h | 39 +- .../core/continuous_contact_manager.h | 2 +- .../core/convex_decomposition.h | 9 +- .../core/discrete_contact_manager.h | 2 +- .../include/tesseract_collision/core/fwd.h | 62 + .../tesseract_collision/core/serialization.h | 12 +- .../include/tesseract_collision/core/types.h | 18 +- .../include/tesseract_collision/core/utils.h | 2 - .../src/contact_managers_plugin_factory.cpp | 14 +- .../core/src/continuous_contact_manager.cpp | 4 +- .../core/src/discrete_contact_manager.cpp | 2 + .../core/src/serialization.cpp | 5 + .../tesseract_collision/fcl/fcl_factories.h | 3 +- tesseract_collision/fcl/src/fcl_factories.cpp | 7 +- tesseract_collision/fcl/src/fcl_utils.cpp | 2 +- .../contact_managers_factory_static_unit.cpp | 1 + .../test/contact_managers_factory_unit.cpp | 2 + .../collision_compound_compound_unit.hpp | 5 +- .../collision_octomap_mesh_unit.hpp | 3 +- .../collision_octomap_octomap_unit.hpp | 4 +- .../collision_octomap_sphere_unit.hpp | 3 +- .../vhacd/convex_decomposition_vhacd.h | 4 +- .../vhacd/src/convex_decomposition_vhacd.cpp | 4 +- .../vhacd/src/create_convex_decomposition.cpp | 4 +- tesseract_common/CMakeLists.txt | 5 +- .../allowed_collision_matrix.h | 55 +- .../tesseract_common/calibration_info.h | 79 + .../tesseract_common/collision_margin_data.h | 123 +- .../include/tesseract_common/eigen_types.h | 37 + .../include/tesseract_common/filesystem.h | 40 + .../include/tesseract_common/fwd.h | 75 + .../include/tesseract_common/joint_state.h | 4 - .../include/tesseract_common/plugin_info.h | 205 ++ .../tesseract_common/resource_locator.h | 11 +- .../include/tesseract_common/serialization.h | 2 +- .../include/tesseract_common/timer.h | 22 +- .../include/tesseract_common/types.h | 236 +- .../include/tesseract_common/utils.h | 29 +- .../include/tesseract_common/yaml_utils.h | 4 +- .../src/allowed_collision_matrix.cpp | 54 + tesseract_common/src/calibration_info.cpp | 72 + .../src/collision_margin_data.cpp | 124 + tesseract_common/src/plugin_info.cpp | 303 ++ tesseract_common/src/resource_locator.cpp | 5 + tesseract_common/src/timer.cpp | 53 + tesseract_common/src/types.cpp | 303 -- tesseract_common/src/utils.cpp | 25 +- .../tesseract_common_serialization_unit.cpp | 3 + .../test/tesseract_common_unit.cpp | 27 +- tesseract_environment/CMakeLists.txt | 9 +- .../include/tesseract_environment/command.h | 4 +- ...add_contact_managers_plugin_info_command.h | 3 +- .../add_kinematics_information_command.h | 1 - .../commands/add_link_command.h | 12 +- .../commands/add_scene_graph_command.h | 11 +- .../commands/add_trajectory_link_command.h | 2 - .../change_collision_margins_command.h | 1 - ...change_joint_acceleration_limits_command.h | 1 - .../commands/change_joint_origin_command.h | 1 - .../change_joint_position_limits_command.h | 1 - .../change_joint_velocity_limits_command.h | 1 - .../change_link_collision_enabled_command.h | 1 - .../commands/change_link_origin_command.h | 1 - .../commands/change_link_visibility_command.h | 1 - .../modify_allowed_collisions_command.h | 1 - .../commands/move_joint_command.h | 1 - .../commands/move_link_command.h | 7 +- .../remove_allowed_collision_link_command.h | 1 - .../commands/remove_joint_command.h | 1 - .../commands/remove_link_command.h | 1 - .../commands/replace_joint_command.h | 7 +- ...ctive_continuous_contact_manager_command.h | 1 - ..._active_discrete_contact_manager_command.h | 1 - .../tesseract_environment/environment.h | 303 +- .../tesseract_environment/environment_cache.h | 17 +- .../environment_monitor.h | 33 +- .../environment_monitor_interface.h | 34 +- .../include/tesseract_environment/events.h | 33 +- .../include/tesseract_environment/fwd.h | 63 + .../include/tesseract_environment/utils.h | 18 +- tesseract_environment/src/command.cpp | 1 + .../src/commands/add_link_command.cpp | 8 +- .../src/commands/add_scene_graph_command.cpp | 11 +- .../src/commands/move_link_command.cpp | 3 +- .../src/commands/replace_joint_command.cpp | 3 +- tesseract_environment/src/environment.cpp | 2618 ++++++++++------- .../src/environment_cache.cpp | 15 +- .../src/environment_monitor.cpp | 55 + .../src/environment_monitor_interface.cpp | 32 + tesseract_environment/src/events.cpp | 45 + tesseract_environment/src/utils.cpp | 10 +- .../test/tesseract_environment_cache_unit.cpp | 16 + .../test/tesseract_environment_collision.cpp | 18 +- ...sseract_environment_serialization_unit.cpp | 19 +- .../test/tesseract_environment_unit.cpp | 24 +- .../test/tesseract_environment_utils.cpp | 14 + tesseract_geometry/CMakeLists.txt | 1 + .../examples/create_geometries_example.cpp | 3 +- .../include/tesseract_geometry/fwd.h | 48 + .../include/tesseract_geometry/geometry.h | 6 +- .../include/tesseract_geometry/impl/box.h | 10 +- .../include/tesseract_geometry/impl/capsule.h | 8 +- .../include/tesseract_geometry/impl/cone.h | 8 +- .../tesseract_geometry/impl/convex_mesh.h | 47 +- .../tesseract_geometry/impl/cylinder.h | 8 +- .../include/tesseract_geometry/impl/mesh.h | 76 +- .../tesseract_geometry/impl/mesh_material.h | 54 +- .../include/tesseract_geometry/impl/octree.h | 168 +- .../tesseract_geometry/impl/octree_utils.h | 60 + .../include/tesseract_geometry/impl/plane.h | 12 +- .../tesseract_geometry/impl/polygon_mesh.h | 89 +- .../tesseract_geometry/impl/sdf_mesh.h | 46 +- .../include/tesseract_geometry/impl/sphere.h | 6 +- .../include/tesseract_geometry/utils.h | 4 +- tesseract_geometry/src/geometries/box.cpp | 8 + tesseract_geometry/src/geometries/capsule.cpp | 7 + tesseract_geometry/src/geometries/cone.cpp | 7 + .../src/geometries/convex_mesh.cpp | 54 +- .../src/geometries/cylinder.cpp | 7 + tesseract_geometry/src/geometries/mesh.cpp | 80 +- .../src/geometries/mesh_material.cpp | 61 + tesseract_geometry/src/geometries/octree.cpp | 121 +- tesseract_geometry/src/geometries/plane.cpp | 9 + .../src/geometries/polygon_mesh.cpp | 82 + .../src/geometries/sdf_mesh.cpp | 54 +- tesseract_geometry/src/geometries/sphere.cpp | 6 + tesseract_geometry/src/geometry.cpp | 2 + tesseract_geometry/src/utils.cpp | 1 + .../tesseract_geometry_serialization_unit.cpp | 12 +- .../test/tesseract_geometry_unit.cpp | 14 +- tesseract_kinematics/core/CMakeLists.txt | 3 + .../core/forward_kinematics.h | 7 +- .../include/tesseract_kinematics/core/fwd.h | 48 + .../core/inverse_kinematics.h | 5 +- .../tesseract_kinematics/core/joint_group.h | 12 +- .../core/kinematic_group.h | 14 +- .../core/kinematics_plugin_factory.h | 77 +- .../tesseract_kinematics/core/rep_factory.h | 10 +- .../tesseract_kinematics/core/rep_inv_kin.h | 19 +- .../tesseract_kinematics/core/rop_factory.h | 10 +- .../tesseract_kinematics/core/rop_inv_kin.h | 18 +- .../include/tesseract_kinematics/core/types.h | 4 +- .../include/tesseract_kinematics/core/utils.h | 229 +- .../tesseract_kinematics/core/validate.h | 3 +- .../core/src/forward_kinematics.cpp | 32 + .../core/src/inverse_kinematics.cpp | 32 + tesseract_kinematics/core/src/joint_group.cpp | 8 + .../core/src/kinematic_group.cpp | 13 +- .../core/src/kinematics_plugin_factory.cpp | 15 +- tesseract_kinematics/core/src/rep_factory.cpp | 13 +- tesseract_kinematics/core/src/rep_inv_kin.cpp | 14 +- tesseract_kinematics/core/src/rop_factory.cpp | 13 +- tesseract_kinematics/core/src/rop_inv_kin.cpp | 14 +- tesseract_kinematics/core/src/utils.cpp | 239 ++ tesseract_kinematics/core/src/validate.cpp | 3 +- .../ikfast/ikfast_factory_boilerplate.h | 1 + .../tesseract_kinematics/kdl/kdl_factories.h | 40 +- .../kdl/kdl_fwd_kin_chain.h | 5 - .../kdl/kdl_inv_kin_chain_lma.h | 6 - .../kdl/kdl_inv_kin_chain_nr.h | 5 - .../kdl/kdl_inv_kin_chain_nr_jl.h | 3 - .../tesseract_kinematics/kdl/kdl_utils.h | 9 +- .../kdl/src/kdl_factories.cpp | 47 +- .../kdl/src/kdl_fwd_kin_chain.cpp | 3 +- .../kdl/src/kdl_inv_kin_chain_lma.cpp | 5 +- .../kdl/src/kdl_inv_kin_chain_nr.cpp | 5 +- .../kdl/src/kdl_inv_kin_chain_nr_jl.cpp | 5 +- tesseract_kinematics/kdl/src/kdl_utils.cpp | 8 + .../tesseract_kinematics/opw/opw_factory.h | 10 +- .../tesseract_kinematics/opw/opw_inv_kin.h | 1 - tesseract_kinematics/opw/src/opw_factory.cpp | 13 +- .../test/kinematics_test_utils.h | 2 + .../tesseract_kinematics/ur/ur_factory.h | 11 +- tesseract_kinematics/ur/src/ur_factory.cpp | 13 +- .../examples/build_scene_graph_example.cpp | 2 + .../include/tesseract_scene_graph/fwd.h | 56 + .../include/tesseract_scene_graph/graph.h | 212 +- .../include/tesseract_scene_graph/joint.h | 236 +- .../tesseract_scene_graph/kdl_parser.h | 14 +- .../include/tesseract_scene_graph/link.h | 116 +- .../tesseract_scene_graph/scene_state.h | 10 +- tesseract_scene_graph/src/graph.cpp | 202 +- tesseract_scene_graph/src/joint.cpp | 196 ++ tesseract_scene_graph/src/kdl_parser.cpp | 12 +- tesseract_scene_graph/src/link.cpp | 81 + tesseract_scene_graph/src/scene_state.cpp | 2 + .../test/tesseract_scene_graph_joint_unit.cpp | 1 + .../test/tesseract_scene_graph_link_unit.cpp | 6 +- .../test/tesseract_scene_graph_unit.cpp | 2 + tesseract_srdf/CMakeLists.txt | 8 +- .../examples/parse_srdf_example.cpp | 3 + .../tesseract_srdf/collision_margins.h | 11 +- .../include/tesseract_srdf/configs.h | 8 +- .../tesseract_srdf/disabled_collisions.h | 11 +- tesseract_srdf/include/tesseract_srdf/fwd.h | 35 + .../include/tesseract_srdf/group_states.h | 5 +- .../tesseract_srdf/group_tool_center_points.h | 5 +- .../include/tesseract_srdf/groups.h | 5 +- .../tesseract_srdf/kinematics_information.h | 6 +- .../include/tesseract_srdf/srdf_model.h | 14 +- tesseract_srdf/include/tesseract_srdf/utils.h | 8 +- tesseract_srdf/src/collision_margins.cpp | 5 +- tesseract_srdf/src/configs.cpp | 4 +- tesseract_srdf/src/disabled_collisions.cpp | 4 +- tesseract_srdf/src/group_states.cpp | 2 +- .../src/group_tool_center_points.cpp | 5 +- tesseract_srdf/src/groups.cpp | 9 +- tesseract_srdf/src/kinematics_information.cpp | 1 + tesseract_srdf/src/srdf_model.cpp | 13 +- tesseract_srdf/src/utils.cpp | 3 + .../tesseract_srdf_serialization_unit.cpp | 3 + tesseract_srdf/test/tesseract_srdf_unit.cpp | 5 + tesseract_state_solver/CMakeLists.txt | 21 +- .../include/tesseract_state_solver/fwd.h | 35 + .../kdl/kdl_state_solver.h | 2 + .../mutable_state_solver.h | 2 +- .../tesseract_state_solver/ofkt/ofkt_node.h | 4 +- .../tesseract_state_solver/ofkt/ofkt_nodes.h | 10 +- .../ofkt/ofkt_state_solver.h | 31 +- .../tesseract_state_solver/state_solver.h | 8 +- .../src/kdl_state_solver.cpp | 3 + .../src/mutable_state_solver.cpp | 32 + tesseract_state_solver/src/ofkt_nodes.cpp | 11 +- .../src/ofkt_state_solver.cpp | 31 +- tesseract_state_solver/src/state_solver.cpp | 32 + .../test/state_solver_test_suite.h | 3 + .../tesseract_support_resource_locator.h | 1 - .../tesseract_support_resource_locator.cpp | 1 + tesseract_urdf/include/tesseract_urdf/box.h | 6 +- .../include/tesseract_urdf/calibration.h | 6 +- .../include/tesseract_urdf/capsule.h | 6 +- .../include/tesseract_urdf/collision.h | 11 +- tesseract_urdf/include/tesseract_urdf/cone.h | 6 +- .../include/tesseract_urdf/convex_mesh.h | 13 +- .../include/tesseract_urdf/cylinder.h | 6 +- .../include/tesseract_urdf/dynamics.h | 6 +- .../include/tesseract_urdf/geometry.h | 13 +- .../include/tesseract_urdf/inertial.h | 6 +- tesseract_urdf/include/tesseract_urdf/joint.h | 6 +- .../include/tesseract_urdf/limits.h | 6 +- tesseract_urdf/include/tesseract_urdf/link.h | 13 +- .../include/tesseract_urdf/material.h | 6 +- tesseract_urdf/include/tesseract_urdf/mesh.h | 13 +- tesseract_urdf/include/tesseract_urdf/mimic.h | 6 +- .../include/tesseract_urdf/octomap.h | 13 +- .../include/tesseract_urdf/octree.h | 10 +- .../include/tesseract_urdf/point_cloud.h | 10 +- .../tesseract_urdf/safety_controller.h | 6 +- .../include/tesseract_urdf/sdf_mesh.h | 12 +- .../include/tesseract_urdf/sphere.h | 6 +- .../include/tesseract_urdf/urdf_parser.h | 20 +- tesseract_urdf/include/tesseract_urdf/utils.h | 4 +- .../include/tesseract_urdf/visual.h | 12 +- tesseract_urdf/src/collision.cpp | 1 + tesseract_urdf/src/dynamics.cpp | 2 +- tesseract_urdf/src/geometry.cpp | 2 +- tesseract_urdf/src/limits.cpp | 2 +- tesseract_urdf/src/mimic.cpp | 2 +- tesseract_urdf/src/octomap.cpp | 14 +- tesseract_urdf/src/octree.cpp | 4 +- tesseract_urdf/src/origin.cpp | 2 +- tesseract_urdf/src/point_cloud.cpp | 8 +- tesseract_urdf/src/urdf_parser.cpp | 15 +- tesseract_urdf/src/utils.cpp | 1 + tesseract_urdf/src/visual.cpp | 3 +- .../test/tesseract_urdf_calibration_unit.cpp | 1 + .../test/tesseract_urdf_common_unit.h | 1 + .../test/tesseract_urdf_dynamics_unit.cpp | 1 + .../test/tesseract_urdf_geometry_unit.cpp | 5 +- .../test/tesseract_urdf_joint_unit.cpp | 1 + .../test/tesseract_urdf_limits_unit.cpp | 1 + .../tesseract_urdf_mesh_material_unit.cpp | 1 + .../test/tesseract_urdf_mimic_unit.cpp | 1 + .../test/tesseract_urdf_octree_unit.cpp | 26 +- .../tesseract_urdf_safety_controller_unit.cpp | 1 + .../test/tesseract_urdf_urdf_unit.cpp | 2 + .../include/tesseract_visualization/fwd.h | 12 + .../ignition/conversions.h | 4 +- .../tesseract_ignition_visualization.h | 2 +- .../trajectory_player.h | 7 +- .../tesseract_visualization/visualization.h | 19 +- .../visualization_loader.h | 10 +- .../src/ignition/conversions.cpp | 1 + .../tesseract_ignition_visualization.cpp | 9 +- .../src/trajectory_player.cpp | 1 + .../src/visualization_loader.cpp | 3 +- 289 files changed, 5877 insertions(+), 3894 deletions(-) create mode 100644 tesseract_collision/core/include/tesseract_collision/core/fwd.h create mode 100644 tesseract_common/include/tesseract_common/calibration_info.h create mode 100644 tesseract_common/include/tesseract_common/eigen_types.h create mode 100644 tesseract_common/include/tesseract_common/filesystem.h create mode 100644 tesseract_common/include/tesseract_common/fwd.h create mode 100644 tesseract_common/include/tesseract_common/plugin_info.h create mode 100644 tesseract_common/src/calibration_info.cpp create mode 100644 tesseract_common/src/plugin_info.cpp create mode 100644 tesseract_common/src/timer.cpp create mode 100644 tesseract_environment/include/tesseract_environment/fwd.h create mode 100644 tesseract_environment/src/environment_monitor.cpp create mode 100644 tesseract_environment/src/environment_monitor_interface.cpp create mode 100644 tesseract_environment/src/events.cpp create mode 100644 tesseract_geometry/include/tesseract_geometry/fwd.h create mode 100644 tesseract_geometry/include/tesseract_geometry/impl/octree_utils.h create mode 100644 tesseract_geometry/src/geometries/mesh_material.cpp create mode 100644 tesseract_kinematics/core/include/tesseract_kinematics/core/fwd.h create mode 100644 tesseract_kinematics/core/src/forward_kinematics.cpp create mode 100644 tesseract_kinematics/core/src/inverse_kinematics.cpp create mode 100644 tesseract_kinematics/core/src/utils.cpp create mode 100644 tesseract_scene_graph/include/tesseract_scene_graph/fwd.h create mode 100644 tesseract_srdf/include/tesseract_srdf/fwd.h create mode 100644 tesseract_state_solver/include/tesseract_state_solver/fwd.h create mode 100644 tesseract_state_solver/src/mutable_state_solver.cpp create mode 100644 tesseract_state_solver/src/state_solver.cpp create mode 100644 tesseract_visualization/include/tesseract_visualization/fwd.h 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..88d561c3d14 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 @@ -59,14 +59,19 @@ class DiscreteContactManagerFactory using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - virtual ~DiscreteContactManagerFactory() = default; + DiscreteContactManagerFactory() = default; + virtual ~DiscreteContactManagerFactory(); + DiscreteContactManagerFactory(const DiscreteContactManagerFactory&) = default; + DiscreteContactManagerFactory& operator=(const DiscreteContactManagerFactory&) = default; + DiscreteContactManagerFactory(DiscreteContactManagerFactory&&) = default; + DiscreteContactManagerFactory& operator=(DiscreteContactManagerFactory&&) = default; /** * @brief Create Discrete Contact Manager Object * @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; @@ -80,14 +85,20 @@ class ContinuousContactManagerFactory using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - virtual ~ContinuousContactManagerFactory() = default; + ContinuousContactManagerFactory() = default; + virtual ~ContinuousContactManagerFactory(); + ContinuousContactManagerFactory(const ContinuousContactManagerFactory&) = default; + ContinuousContactManagerFactory& operator=(const ContinuousContactManagerFactory&) = default; + ContinuousContactManagerFactory(ContinuousContactManagerFactory&&) = default; + ContinuousContactManagerFactory& operator=(ContinuousContactManagerFactory&&) = default; /** * @brief Create Inverse Kinematics Object * @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 +248,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/continuous_contact_manager.h b/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h index 0ce61057d93..3eaf54380f1 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h +++ b/tesseract_collision/core/include/tesseract_collision/core/continuous_contact_manager.h @@ -48,7 +48,7 @@ class ContinuousContactManager using ConstUPtr = std::unique_ptr; ContinuousContactManager() = default; - virtual ~ContinuousContactManager() = default; + virtual ~ContinuousContactManager(); ContinuousContactManager(const ContinuousContactManager&) = delete; ContinuousContactManager& operator=(const ContinuousContactManager&) = delete; ContinuousContactManager(ContinuousContactManager&&) = delete; 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/discrete_contact_manager.h b/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h index 38d59ae37e7..7dcf1f0fe4c 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h +++ b/tesseract_collision/core/include/tesseract_collision/core/discrete_contact_manager.h @@ -50,7 +50,7 @@ class DiscreteContactManager using ConstUPtr = std::unique_ptr; DiscreteContactManager() = default; - virtual ~DiscreteContactManager() = default; + virtual ~DiscreteContactManager(); DiscreteContactManager(const DiscreteContactManager&) = delete; DiscreteContactManager& operator=(const DiscreteContactManager&) = delete; DiscreteContactManager(DiscreteContactManager&&) = delete; 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..2249076e0b8 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 @@ -44,6 +46,9 @@ namespace tesseract_collision const std::string DiscreteContactManagerFactory::SECTION_NAME = "DiscColl"; const std::string ContinuousContactManagerFactory::SECTION_NAME = "ContColl"; +DiscreteContactManagerFactory::~DiscreteContactManagerFactory() = default; +ContinuousContactManagerFactory::~ContinuousContactManagerFactory() = default; + ContactManagersPluginFactory::ContactManagersPluginFactory() { plugin_loader_.search_libraries_env = TESSERACT_CONTACT_MANAGERS_PLUGINS_ENV; @@ -202,7 +207,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 +222,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 +250,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 +265,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..45b8813c554 100644 --- a/tesseract_collision/core/src/continuous_contact_manager.cpp +++ b/tesseract_collision/core/src/continuous_contact_manager.cpp @@ -24,11 +24,13 @@ * limitations under the License. */ -#include +#include #include namespace tesseract_collision { +ContinuousContactManager::~ContinuousContactManager() = default; + void ContinuousContactManager::applyContactManagerConfig(const ContactManagerConfig& config) { setCollisionMarginData(config.margin_data, config.margin_data_override_type); diff --git a/tesseract_collision/core/src/discrete_contact_manager.cpp b/tesseract_collision/core/src/discrete_contact_manager.cpp index ebb69522c43..fbd174ff689 100644 --- a/tesseract_collision/core/src/discrete_contact_manager.cpp +++ b/tesseract_collision/core/src/discrete_contact_manager.cpp @@ -29,6 +29,8 @@ namespace tesseract_collision { +DiscreteContactManager::~DiscreteContactManager() = default; + void DiscreteContactManager::applyContactManagerConfig(const ContactManagerConfig& config) { setCollisionMarginData(config.margin_data, config.margin_data_override_type); 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/resource_locator.h b/tesseract_common/include/tesseract_common/resource_locator.h index 22a9d30f416..f364f707be3 100644 --- a/tesseract_common/include/tesseract_common/resource_locator.h +++ b/tesseract_common/include/tesseract_common/resource_locator.h @@ -48,7 +48,12 @@ class ResourceLocator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - virtual ~ResourceLocator() = default; + ResourceLocator() = default; + virtual ~ResourceLocator(); + ResourceLocator(const ResourceLocator&) = default; + ResourceLocator& operator=(const ResourceLocator&) = default; + ResourceLocator(ResourceLocator&&) = default; + ResourceLocator& operator=(ResourceLocator&&) = default; /** * @brief Locate a resource based on a URL @@ -82,7 +87,7 @@ class GeneralResourceLocator : public ResourceLocator GeneralResourceLocator& operator=(const GeneralResourceLocator&) = default; GeneralResourceLocator(GeneralResourceLocator&&) = default; GeneralResourceLocator& operator=(GeneralResourceLocator&&) = default; - ~GeneralResourceLocator() override = default; + ~GeneralResourceLocator() override; std::shared_ptr locateResource(const std::string& url) const override; @@ -208,7 +213,7 @@ class BytesResource : public tesseract_common::Resource BytesResource(std::string url, std::vector bytes, ResourceLocator::ConstPtr parent = nullptr); BytesResource(std::string url, const uint8_t* bytes, size_t bytes_len, ResourceLocator::ConstPtr parent = nullptr); - ~BytesResource() override = default; + ~BytesResource() override; BytesResource(const BytesResource&) = default; BytesResource& operator=(const BytesResource&) = default; BytesResource(BytesResource&&) = default; 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/resource_locator.cpp b/tesseract_common/src/resource_locator.cpp index c874200c4d7..44914ad212e 100644 --- a/tesseract_common/src/resource_locator.cpp +++ b/tesseract_common/src/resource_locator.cpp @@ -42,6 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_common { +ResourceLocator::~ResourceLocator() = default; bool ResourceLocator::operator==(const ResourceLocator& /*rhs*/) const { return true; } bool ResourceLocator::operator!=(const ResourceLocator& /*rhs*/) const { return false; } @@ -50,6 +51,8 @@ void ResourceLocator::serialize(Archive& /*ar*/, const unsigned int /*version*/) { } +GeneralResourceLocator::~GeneralResourceLocator() = default; + GeneralResourceLocator::GeneralResourceLocator() { // This was added to allow user defined resource path @@ -275,6 +278,8 @@ BytesResource::BytesResource(std::string url, const uint8_t* bytes, size_t bytes { } +BytesResource::~BytesResource() = default; + bool BytesResource::isFile() const { return false; } std::string BytesResource::getUrl() const { return url_; } std::string BytesResource::getFilePath() const { return ""; } 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 814095f0a02..2f947b38f45 100644 --- a/tesseract_common/src/types.cpp +++ b/tesseract_common/src/types.cpp @@ -27,20 +27,9 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include -#include -#include -#include -#include -#include -#include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include #include -#include namespace tesseract_common { @@ -70,296 +59,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..dc01a4b4a99 100644 --- a/tesseract_environment/CMakeLists.txt +++ b/tesseract_environment/CMakeLists.txt @@ -47,7 +47,14 @@ 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_interface.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..e9b928c5a37 100644 --- a/tesseract_environment/include/tesseract_environment/command.h +++ b/tesseract_environment/include/tesseract_environment/command.h @@ -80,7 +80,7 @@ class Command using ConstPtr = std::shared_ptr; Command(CommandType type = CommandType::UNINITIALIZED); - virtual ~Command() = default; + virtual ~Command(); Command(const Command&) = default; Command& operator=(const Command&) = default; Command(Command&&) = default; @@ -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..de04845b4e3 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/tesseract_environment/include/tesseract_environment/environment.h @@ -25,32 +25,31 @@ */ #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 +#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 +#include namespace tesseract_environment { @@ -61,6 +60,8 @@ namespace tesseract_environment */ using FindTCPOffsetCallbackFn = std::function; +using EventCallbackFn = std::function; + class Environment { public: @@ -74,8 +75,8 @@ class Environment using ConstUPtr = std::unique_ptr; /** @brief Default constructor */ - Environment() = default; - virtual ~Environment() = default; + Environment(); + virtual ~Environment(); Environment(const Environment&) = delete; Environment& operator=(const Environment&) = delete; Environment(Environment&&) = delete; @@ -89,7 +90,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 +101,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 +145,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 +153,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 +161,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 +181,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 +189,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 +199,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, + const std::string& ik_solver_name = "") const; /** * @brief Find tool center point provided in the manipulator info @@ -257,14 +259,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 +313,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 +345,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 +446,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 +458,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 +474,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 +483,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 +493,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 +502,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; @@ -520,205 +523,13 @@ class Environment bool operator==(const Environment& rhs) const; bool operator!=(const Environment& rhs) const; -protected: - /** - * @brief Identifies if the object has been initialized - * @note This is intentionally not serialized it will auto updated - */ - bool initialized_{ false }; - - /** - * @brief This increments when the scene graph is modified - * @note This is intentionally not serialized it will auto updated - */ - int revision_{ 0 }; - - /** @brief This is the revision number after initialization used when reset is called */ - int init_revision_{ 0 }; - - /** @brief The history of commands applied to the environment after initialization */ - Commands commands_{}; - - /** - * @brief Tesseract Scene Graph - * @note This is intentionally not serialized it will auto updated - */ - tesseract_scene_graph::SceneGraph::Ptr scene_graph_{ nullptr }; - - /** - * @brief The kinematics information - * @note This is intentionally not serialized it will auto updated - */ - tesseract_srdf::KinematicsInformation kinematics_information_; - - /** - * @brief The kinematics factory - * @note This is intentionally not serialized it will auto updated - */ - tesseract_kinematics::KinematicsPluginFactory kinematics_factory_; - - /** @brief Current state of the environment */ - tesseract_scene_graph::SceneState current_state_; - - /** @brief Environment timestamp */ - std::chrono::system_clock::time_point timestamp_{ std::chrono::system_clock::now() }; - - /** @brief Current state timestamp */ - std::chrono::system_clock::time_point current_state_timestamp_{ std::chrono::system_clock::now() }; - - /** - * @brief Tesseract State Solver - * @note This is intentionally not serialized it will auto updated - */ - tesseract_scene_graph::MutableStateSolver::UPtr 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_; - - /** - * @brief A vector of user defined callbacks for locating tool center point - * @todo This needs to be switched to class so it may be serialized - */ - std::vector find_tcp_cb_{}; - - /** - * @brief A map of user defined event callback functions - * @details This should not be cloned or serialized - */ - 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 }; - - /** - * @brief The contact manager information - * @note This is intentionally not serialized it will auto updated - */ - tesseract_common::ContactManagersPluginInfo contact_managers_plugin_info_; - - /** - * @brief The contact managers factory - * @note This is intentionally not serialized it will auto updated - */ - tesseract_collision::ContactManagersPluginFactory contact_managers_factory_; - - /** - * @brief The collision margin data - * @note This is intentionally not serialized it will auto updated - */ - tesseract_collision::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::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::shared_mutex continuous_manager_mutex_; - - /** - * @brief A cache of group joint names to provide faster access - * @details This will cleared when environment changes - * @note This is intentionally not serialized it will auto updated - */ - mutable std::unordered_map> group_joint_names_cache_{}; - mutable std::shared_mutex group_joint_names_cache_mutex_; - - /** - * @brief A cache of joint groups to provide faster access - * @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::shared_mutex joint_group_cache_mutex_; - - /** - * @brief A cache of kinematic groups to provide faster access - * @details This will cleared when environment changes - * @note This is intentionally not serialized it will auto updated - */ - mutable std::map, tesseract_kinematics::KinematicGroup::UPtr> - kinematic_group_cache_{}; - mutable std::shared_mutex kinematic_group_cache_mutex_; - +private: /** @brief The environment can be accessed from multiple threads, need use mutex throughout */ mutable std::shared_mutex mutex_; - /** This will update the contact managers transforms */ - void currentStateChanged(); - - /** This will notify the state solver that the environment has changed */ - void environmentChanged(); - - /** - * @brief @brief Passes a current state changed event to the callbacks - * @note This does not take a lock - */ - void triggerCurrentStateChangedCallbacks(); - - /** - * @brief Passes a environment changed event to the callbacks - * @note This does not take a lock - */ - void triggerEnvironmentChangedCallbacks(); - -private: - bool removeLinkHelper(const std::string& name); - - static void getCollisionObject(tesseract_collision::CollisionShapesConst& 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; - - tesseract_collision::ContinuousContactManager::UPtr 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); - - /** @brief Apply Command Helper which does not lock */ - bool applyCommandsHelper(const Commands& 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 replace_allowed); + /** @brief This hides specific implemenation details to allow forward declarations */ + struct Implementation; + std::unique_ptr impl_; friend class boost::serialization::access; template @@ -729,6 +540,10 @@ class Environment template void serialize(Archive& ar, const unsigned int version); // NOLINT + +public: + /** @brief This should only be used by the clone method */ + explicit Environment(std::unique_ptr impl); }; } // namespace tesseract_environment diff --git a/tesseract_environment/include/tesseract_environment/environment_cache.h b/tesseract_environment/include/tesseract_environment/environment_cache.h index b88e9faf256..e0140831bb2 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: @@ -43,8 +44,8 @@ class EnvironmentCache using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - EnvironmentCache() = default; - virtual ~EnvironmentCache() = default; + EnvironmentCache(); + virtual ~EnvironmentCache(); EnvironmentCache(const EnvironmentCache&) = delete; EnvironmentCache& operator=(const EnvironmentCache&) = delete; EnvironmentCache(EnvironmentCache&&) = delete; @@ -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..aa9d77e5dd0 100644 --- a/tesseract_environment/include/tesseract_environment/environment_monitor.h +++ b/tesseract_environment/include/tesseract_environment/environment_monitor.h @@ -26,11 +26,17 @@ #ifndef TESSERACT_ENVIRONMENT_ENVIRONMENT_MONITOR_H #define TESSERACT_ENVIRONMENT_ENVIRONMENT_MONITOR_H -#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_environment { +class Environment; + enum class MonitoredEnvironmentMode : int { /** @@ -70,22 +76,23 @@ 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; + virtual ~EnvironmentMonitor(); + EnvironmentMonitor(const EnvironmentMonitor&) = default; + EnvironmentMonitor& operator=(const EnvironmentMonitor&) = default; + EnvironmentMonitor(EnvironmentMonitor&&) = default; + EnvironmentMonitor& operator=(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 +101,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 +115,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 +192,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..233c772425c 100644 --- a/tesseract_environment/include/tesseract_environment/environment_monitor_interface.h +++ b/tesseract_environment/include/tesseract_environment/environment_monitor_interface.h @@ -29,13 +29,18 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include namespace tesseract_environment { +class Command; +class Environment; + class EnvironmentMonitorInterface { public: @@ -44,9 +49,12 @@ class EnvironmentMonitorInterface using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - EnvironmentMonitorInterface(std::string env_name) : env_name_(std::move(env_name)) {} - - virtual ~EnvironmentMonitorInterface() = default; + explicit EnvironmentMonitorInterface(std::string env_name); + virtual ~EnvironmentMonitorInterface(); + EnvironmentMonitorInterface(const EnvironmentMonitorInterface&) = default; + EnvironmentMonitorInterface& operator=(const EnvironmentMonitorInterface&) = default; + EnvironmentMonitorInterface(EnvironmentMonitorInterface&&) = default; + EnvironmentMonitorInterface& operator=(EnvironmentMonitorInterface&&) = default; /** * @brief This will wait for all namespaces to begin publishing @@ -81,9 +89,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 +99,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 +140,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..7e6f580f655 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,8 +47,12 @@ enum class Events /** @brief The event base class */ struct Event { - Event(Events type) : type(type) {} - virtual ~Event() = default; + Event(Events type); + virtual ~Event(); + Event(const Event&) = default; + Event& operator=(const Event&) = delete; + Event(Event&&) = default; + Event& operator=(Event&&) = delete; const Events type; }; @@ -51,12 +63,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 +75,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..7681a1c8315 --- /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; +struct 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/command.cpp b/tesseract_environment/src/command.cpp index 6509001aeeb..a4bbdd3d05d 100644 --- a/tesseract_environment/src/command.cpp +++ b/tesseract_environment/src/command.cpp @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_environment { Command::Command(CommandType type) : type_(type) {} +Command::~Command() = default; template void save(Archive& ar, const CommandType& g, const unsigned int /*version*/) 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..fe2b5f71880 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -25,275 +25,68 @@ */ #include -#include -#include -#include -#include -#include - TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -namespace tesseract_environment -{ -bool Environment::initHelper(const Commands& commands) -{ - if (commands.empty()) - return false; - - if (commands.at(0)->getType() != CommandType::ADD_SCENE_GRAPH) - { - CONSOLE_BRIDGE_logError("When initializing environment from command history the first command must be type " - "ADD_SCENE_GRAPH!"); - return false; - } - - clear(); - - scene_graph_ = std::make_shared( - std::static_pointer_cast(commands.at(0))->getSceneGraph()->getName()); - - is_contact_allowed_fn_ = [this](const std::string& l1, const std::string& l2) { - return scene_graph_->isCollisionAllowed(l1, l2); - }; - - if (!applyCommandsHelper(commands)) - { - CONSOLE_BRIDGE_logError("When initializing environment from command history, it failed to apply a command!"); - return false; - } - - initialized_ = true; - init_revision_ = revision_; - - environmentChanged(); - - return initialized_; -} - -bool Environment::init(const Commands& commands) -{ - bool success{ false }; - { - std::unique_lock lock(mutex_); - success = initHelper(commands); - } - - // Call the event callbacks - std::shared_lock lock(mutex_); - triggerEnvironmentChangedCallbacks(); - triggerCurrentStateChangedCallbacks(); - - return success; -} - -bool Environment::init(const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_srdf::SRDFModel::ConstPtr& 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) -{ - { - std::unique_lock lock(mutex_); - resource_locator_ = locator; - } - - // Parse urdf string into Scene Graph - tesseract_scene_graph::SceneGraph::Ptr scene_graph; - try - { - scene_graph = tesseract_urdf::parseURDFString(urdf_string, *resource_locator_); - } - catch (const std::exception& e) - { - CONSOLE_BRIDGE_logError("Failed to parse URDF."); - tesseract_common::printNestedException(e); - return false; - } - - Commands commands = getInitCommands(*scene_graph); - return init(commands); -} - -bool Environment::init(const std::string& urdf_string, - const std::string& srdf_string, - const tesseract_common::ResourceLocator::ConstPtr& locator) -{ - { - std::unique_lock lock(mutex_); - resource_locator_ = locator; - } - - // Parse urdf string into Scene Graph - tesseract_scene_graph::SceneGraph::Ptr scene_graph; - try - { - scene_graph = tesseract_urdf::parseURDFString(urdf_string, *resource_locator_); - } - catch (const std::exception& e) - { - CONSOLE_BRIDGE_logError("Failed to parse URDF."); - tesseract_common::printNestedException(e); - return false; - } +#include +#include +#include +#include - // Parse srdf string into SRDF Model - auto srdf = std::make_shared(); - try - { - srdf->initString(*scene_graph, srdf_string, *resource_locator_); - } - catch (const std::exception& e) - { - CONSOLE_BRIDGE_logError("Failed to parse SRDF."); - tesseract_common::printNestedException(e); - return false; - } +#include +#include +#include +#include - Commands commands = getInitCommands(*scene_graph, srdf); - return init(commands); -} +#include -bool Environment::init(const tesseract_common::fs::path& urdf_path, - const tesseract_common::ResourceLocator::ConstPtr& locator) -{ - { - std::unique_lock lock(mutex_); - resource_locator_ = locator; - } +#include +#include +#include - // Parse urdf file into Scene Graph - tesseract_scene_graph::SceneGraph::Ptr scene_graph; - try - { - scene_graph = tesseract_urdf::parseURDFFile(urdf_path.string(), *resource_locator_); - } - catch (const std::exception& e) - { - CONSOLE_BRIDGE_logError("Failed to parse URDF."); - tesseract_common::printNestedException(e); - return false; - } +#include +#include +#include +#include +#include - Commands commands = getInitCommands(*scene_graph); - return init(commands); -} +#include +#include +#include +#include -bool Environment::init(const tesseract_common::fs::path& urdf_path, - const tesseract_common::fs::path& srdf_path, - const tesseract_common::ResourceLocator::ConstPtr& locator) -{ - { - std::unique_lock lock(mutex_); - resource_locator_ = locator; - } +#include +#include - // Parse urdf file into Scene Graph - tesseract_scene_graph::SceneGraph::Ptr scene_graph; - try - { - scene_graph = tesseract_urdf::parseURDFFile(urdf_path.string(), *resource_locator_); - } - catch (const std::exception& e) - { - CONSOLE_BRIDGE_logError("Failed to parse URDF."); - tesseract_common::printNestedException(e); - return false; - } +#include +#include - // Parse srdf file into SRDF Model - auto srdf = std::make_shared(); - try - { - srdf->initFile(*scene_graph, srdf_path.string(), *resource_locator_); - } - catch (const std::exception& e) - { - CONSOLE_BRIDGE_logError("Failed to parse SRDF."); - tesseract_common::printNestedException(e); - return false; - } +#include +#include - Commands commands = getInitCommands(*scene_graph, srdf); - return init(commands); // NOLINT -} +#include -bool Environment::reset() +namespace tesseract_environment { - bool success{ false }; - { - std::unique_lock lock(mutex_); - - Commands init_command; - if (commands_.empty() || !initialized_) - return false; - - init_command.reserve(static_cast(init_revision_)); - for (std::size_t i = 0; i < static_cast(init_revision_); ++i) - init_command.push_back(commands_[i]); - - success = initHelper(init_command); - } - - // Call the event callbacks - std::shared_lock lock(mutex_); - triggerCurrentStateChangedCallbacks(); - triggerEnvironmentChangedCallbacks(); - - return success; -} - -void Environment::clear() +void getCollisionObject(std::vector>& shapes, + tesseract_common::VectorIsometry3d& shape_poses, + const tesseract_scene_graph::Link& link) { - initialized_ = false; - revision_ = 0; - init_revision_ = 0; - scene_graph_ = nullptr; - state_solver_ = nullptr; - current_state_ = tesseract_scene_graph::SceneState(); - commands_.clear(); - kinematics_information_.clear(); - kinematics_factory_ = tesseract_kinematics::KinematicsPluginFactory(); - is_contact_allowed_fn_ = nullptr; - collision_margin_data_ = tesseract_collision::CollisionMarginData(); - contact_managers_plugin_info_.clear(); - contact_managers_factory_ = tesseract_collision::ContactManagersPluginFactory(); - - { - std::unique_lock lock(discrete_manager_mutex_); - discrete_manager_ = nullptr; - } - - { - std::unique_lock lock(continuous_manager_mutex_); - continuous_manager_ = nullptr; - } - - { - std::unique_lock lock(group_joint_names_cache_mutex_); - group_joint_names_cache_.clear(); - } - - { - std::unique_lock lock(joint_group_cache_mutex_); - joint_group_cache_.clear(); - } - + for (const auto& c : link.collision) { - std::unique_lock lock(kinematic_group_cache_mutex_); - kinematic_group_cache_.clear(); + shapes.push_back(c->geometry); + shape_poses.push_back(c->origin); } } -Commands Environment::getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph, - const tesseract_srdf::SRDFModel::ConstPtr& srdf_model) +std::vector> +getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::shared_ptr& srdf_model = nullptr) { Commands commands; @@ -333,420 +126,463 @@ Commands Environment::getInitCommands(const tesseract_scene_graph::SceneGraph& s return commands; } -bool Environment::isInitialized() const +struct Environment::Implementation { - std::shared_lock lock(mutex_); - return initialized_; -} + ~Implementation() = default; -int Environment::getRevision() const -{ - std::shared_lock lock(mutex_); - return revision_; -} + /** + * @brief Identifies if the object has been initialized + * @note This is intentionally not serialized it will auto updated + */ + bool initialized{ false }; -Commands Environment::getCommandHistory() const -{ - std::shared_lock lock(mutex_); - return commands_; -} + /** + * @brief This increments when the scene graph is modified + * @note This is intentionally not serialized it will auto updated + */ + int revision{ 0 }; -bool Environment::applyCommands(const Commands& commands) -{ - bool success{ false }; - { - std::unique_lock lock(mutex_); - success = applyCommandsHelper(commands); - } - std::shared_lock lock(mutex_); - triggerEnvironmentChangedCallbacks(); - triggerCurrentStateChangedCallbacks(); + /** @brief This is the revision number after initialization used when reset is called */ + int init_revision{ 0 }; - return success; -} + /** @brief The history of commands applied to the environment after initialization */ + std::vector> commands{}; -bool Environment::applyCommand(Command::ConstPtr command) { return applyCommands({ std::move(command) }); } + /** + * @brief Tesseract Scene Graph + * @note This is intentionally not serialized it will auto updated + */ + std::shared_ptr scene_graph{ nullptr }; -tesseract_scene_graph::SceneGraph::ConstPtr Environment::getSceneGraph() const { return scene_graph_; } + /** @brief Current state of the environment */ + tesseract_scene_graph::SceneState current_state; -std::vector Environment::getGroupJointNames(const std::string& group_name) const -{ - std::shared_lock lock(mutex_); - auto it = - std::find(kinematics_information_.group_names.begin(), kinematics_information_.group_names.end(), group_name); + /** @brief Environment timestamp */ + std::chrono::system_clock::time_point timestamp{ std::chrono::system_clock::now() }; + + /** @brief Current state timestamp */ + std::chrono::system_clock::time_point current_state_timestamp{ std::chrono::system_clock::now() }; + + /** + * @brief Tesseract State Solver + * @note This is intentionally not serialized it will auto updated + */ + 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 + */ + std::function is_contact_allowed_fn; + + /** + * @brief A vector of user defined callbacks for locating tool center point + * @todo This needs to be switched to class so it may be serialized + */ + std::vector find_tcp_cb{}; + + /** + * @brief A map of user defined event callback functions + * @details This should not be cloned or serialized + */ + std::map event_cb{}; + + /** @brief Used when initialized by URDF_STRING, URDF_STRING_SRDF_STRING, URDF_PATH, and URDF_PATH_SRDF_PATH */ + std::shared_ptr resource_locator{ nullptr }; + + /** + * @brief The collision margin data + * @note This is intentionally not serialized it will auto updated + */ + tesseract_common::CollisionMarginData collision_margin_data; + + /** + * @brief The kinematics information + * @note This is intentionally not serialized it will auto updated + */ + tesseract_srdf::KinematicsInformation kinematics_information; - if (it == kinematics_information_.group_names.end()) - throw std::runtime_error("Environment, Joint group '" + group_name + "' does not exist!"); + /** + * @brief The kinematics factory + * @note This is intentionally not serialized it will auto updated + */ + tesseract_kinematics::KinematicsPluginFactory kinematics_factory; - std::unique_lock cache_lock(group_joint_names_cache_mutex_); - auto cache_it = group_joint_names_cache_.find(group_name); - if (cache_it != group_joint_names_cache_.end()) - { - CONSOLE_BRIDGE_logDebug("Environment, getGroupJointNames(%s) cache hit!", group_name.c_str()); - return cache_it->second; - } + /** + * @brief The contact manager information + * @note This is intentionally not serialized it will auto updated + */ + tesseract_common::ContactManagersPluginInfo contact_managers_plugin_info; - CONSOLE_BRIDGE_logDebug("Environment, getGroupJointNames(%s) cache miss!", group_name.c_str()); - auto chain_it = kinematics_information_.chain_groups.find(group_name); - if (chain_it != kinematics_information_.chain_groups.end()) - { - if (chain_it->second.size() > 1) - throw std::runtime_error("Environment, Groups with multiple chains is not supported!"); + /** + * @brief The contact managers factory + * @note This is intentionally not serialized it will auto updated + */ + tesseract_collision::ContactManagersPluginFactory contact_managers_factory; - tesseract_scene_graph::ShortestPath path = - scene_graph_->getShortestPath(chain_it->second.begin()->first, chain_it->second.begin()->second); + /** + * @brief The discrete contact manager object + * @note This is intentionally not serialized it will auto updated + */ + mutable std::unique_ptr discrete_manager{ nullptr }; + mutable std::shared_mutex discrete_manager_mutex; - group_joint_names_cache_[group_name] = path.active_joints; - return path.active_joints; - } + /** + * @brief The continuous contact manager object + * @note This is intentionally not serialized it will auto updated + */ + mutable std::unique_ptr continuous_manager{ nullptr }; + mutable std::shared_mutex continuous_manager_mutex; - auto joint_it = kinematics_information_.joint_groups.find(group_name); - if (joint_it != kinematics_information_.joint_groups.end()) - { - group_joint_names_cache_[group_name] = joint_it->second; - return joint_it->second; - } + /** + * @brief A cache of group joint names to provide faster access + * @details This will cleared when environment changes + * @note This is intentionally not serialized it will auto updated + */ + mutable std::unordered_map> group_joint_names_cache{}; + mutable std::shared_mutex group_joint_names_cache_mutex; - auto link_it = kinematics_information_.link_groups.find(group_name); - if (link_it != kinematics_information_.link_groups.end()) - throw std::runtime_error("Environment, Link groups are currently not supported!"); + /** + * @brief A cache of joint groups to provide faster access + * @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::shared_mutex joint_group_cache_mutex; - throw std::runtime_error("Environment, failed to get group '" + group_name + "' joint names!"); -} + /** + * @brief A cache of kinematic groups to provide faster access + * @details This will cleared when environment changes + * @note This is intentionally not serialized it will auto updated + */ + mutable std::map, std::unique_ptr> + kinematic_group_cache{}; + mutable std::shared_mutex kinematic_group_cache_mutex; -tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& group_name) const -{ - std::shared_lock lock(mutex_); + bool operator==(const Implementation& rhs) const; + + bool initHelper(const std::vector>& commands); - std::unique_lock cache_lock(joint_group_cache_mutex_); - auto it = joint_group_cache_.find(group_name); - if (it != joint_group_cache_.end()) - { - CONSOLE_BRIDGE_logDebug("Environment, getJointGroup(%s) cache hit!", group_name.c_str()); - return std::make_unique(*it->second); - } + void setState(const std::unordered_map& joints); + + void setState(const std::vector& joint_names, const Eigen::Ref& joint_values); - CONSOLE_BRIDGE_logDebug("Environment, getJointGroup(%s) cache miss!", group_name.c_str()); - // Store copy in cache and return - std::vector joint_names = getGroupJointNames(group_name); - tesseract_kinematics::JointGroup::UPtr jg = getJointGroup(group_name, joint_names); - joint_group_cache_[group_name] = std::make_unique(*jg); + Eigen::VectorXd getCurrentJointValues() const; - return jg; -} + Eigen::VectorXd getCurrentJointValues(const std::vector& joint_names) const; -tesseract_kinematics::JointGroup::UPtr 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_); -} + std::vector getStaticLinkNames(const std::vector& joint_names) const; -tesseract_kinematics::KinematicGroup::UPtr Environment::getKinematicGroup(const std::string& group_name, - std::string ik_solver_name) const -{ - std::shared_lock lock(mutex_); + void clear(); - std::unique_lock cache_lock(kinematic_group_cache_mutex_); - std::pair key = std::make_pair(group_name, ik_solver_name); - auto it = kinematic_group_cache_.find(key); - if (it != kinematic_group_cache_.end()) - { - CONSOLE_BRIDGE_logDebug( - "Environment, getKinematicGroup(%s, %s) cache hit!", group_name.c_str(), ik_solver_name.c_str()); - return std::make_unique(*it->second); - } + bool reset(); - CONSOLE_BRIDGE_logDebug( - "Environment, getKinematicGroup(%s, %s) cache miss!", group_name.c_str(), ik_solver_name.c_str()); - std::vector joint_names = getGroupJointNames(group_name); + /** This will update the contact managers transforms */ + void currentStateChanged(); - if (ik_solver_name.empty()) - ik_solver_name = kinematics_factory_.getDefaultInvKinPlugin(group_name); + /** This will notify the state solver that the environment has changed */ + void environmentChanged(); - tesseract_kinematics::InverseKinematics::UPtr inv_kin = - kinematics_factory_.createInvKin(group_name, ik_solver_name, *scene_graph_, current_state_); + /** @brief @brief Passes a current state changed event to the callbacks */ + void triggerCurrentStateChangedCallbacks(); - // TODO add error message - if (inv_kin == nullptr) - return nullptr; + /** @brief Passes a environment changed event to the callbacks */ + void triggerEnvironmentChangedCallbacks(); - // Store copy in cache and return - auto kg = std::make_unique( - group_name, joint_names, std::move(inv_kin), *scene_graph_, current_state_); + /** @brief Trigger both Current State and Environment changed callback */ + void triggerCallbacks(); - kinematic_group_cache_[key] = std::make_unique(*kg); + std::vector getGroupJointNames(const std::string& group_name) const; -#ifndef NDEBUG - if (!tesseract_kinematics::checkKinematics(*kg)) - { - CONSOLE_BRIDGE_logError("Check Kinematics failed. This means that inverse kinematics solution for a pose do not " - "match forward kinematics solution. Did you change the URDF recently?"); - } -#endif + std::unique_ptr getJointGroup(const std::string& group_name) const; - return kg; -} + std::unique_ptr getJointGroup(const std::string& name, + const std::vector& joint_names) const; -// NOLINTNEXTLINE -Eigen::Isometry3d Environment::findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const -{ - std::shared_lock lock(mutex_); + std::unique_ptr getKinematicGroup(const std::string& group_name, + std::string ik_solver_name) const; - // If it is already an Isometry3d return the offset - if (manip_info.tcp_offset.index() != 0) - return std::get<1>(manip_info.tcp_offset); + Eigen::Isometry3d findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const; - // Check if the tcp offset name is a link in the scene, if so throw an exception - const std::string& tcp_offset_name = std::get<0>(manip_info.tcp_offset); - if (state_solver_->hasLinkName(tcp_offset_name)) - throw std::runtime_error("The tcp offset name '" + tcp_offset_name + - "' should not be an existing link in the scene. Assign it as the tcp_frame instead!"); + std::unique_ptr getDiscreteContactManager() const; - // Check Manipulator Manager for TCP - if (kinematics_information_.hasGroupTCP(manip_info.manipulator, tcp_offset_name)) - return kinematics_information_.group_tcps.at(manip_info.manipulator).at(tcp_offset_name); + std::unique_ptr getContinuousContactManager() const; - // Check callbacks for TCP Offset - for (const auto& fn : find_tcp_cb_) - { - try - { - Eigen::Isometry3d tcp = fn(manip_info); - return tcp; - } - catch (...) - { - CONSOLE_BRIDGE_logDebug("User Defined Find TCP Callback Failed!"); - } - } + void clearCachedDiscreteContactManager() const; - throw std::runtime_error("Could not find tcp by name " + tcp_offset_name + "'!"); -} + void clearCachedContinuousContactManager() const; -void Environment::addFindTCPOffsetCallback(const FindTCPOffsetCallbackFn& fn) -{ - std::unique_lock lock(mutex_); - find_tcp_cb_.push_back(fn); -} + bool setActiveDiscreteContactManagerHelper(const std::string& name); -std::vector Environment::getFindTCPOffsetCallbacks() const -{ - std::shared_lock lock(mutex_); - return find_tcp_cb_; -} + bool setActiveContinuousContactManagerHelper(const std::string& name); -void Environment::addEventCallback(std::size_t hash, const EventCallbackFn& fn) -{ - std::unique_lock lock(mutex_); - event_cb_[hash] = fn; -} + std::unique_ptr + getDiscreteContactManagerHelper(const std::string& name) const; -void Environment::removeEventCallback(std::size_t hash) -{ - std::unique_lock lock(mutex_); - event_cb_.erase(hash); -} + std::unique_ptr + getContinuousContactManagerHelper(const std::string& name) const; -void Environment::clearEventCallbacks() -{ - std::unique_lock lock(mutex_); - event_cb_.clear(); -} + bool setActiveDiscreteContactManager(const std::string& name); -std::map Environment::getEventCallbacks() const -{ - std::shared_lock lock(mutex_); - return event_cb_; -} + std::unique_ptr getDiscreteContactManager(const std::string& name) const; -void Environment::setResourceLocator(tesseract_common::ResourceLocator::ConstPtr locator) -{ - std::unique_lock lock(mutex_); - resource_locator_ = std::move(locator); -} + bool setActiveContinuousContactManager(const std::string& name); -tesseract_common::ResourceLocator::ConstPtr Environment::getResourceLocator() const -{ - std::shared_lock lock(mutex_); - return resource_locator_; -} + std::unique_ptr + getContinuousContactManager(const std::string& name) const; -void Environment::setName(const std::string& name) -{ - std::unique_lock lock(mutex_); - scene_graph_->setName(name); -} + bool removeLinkHelper(const std::string& name); -const std::string& Environment::getName() const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getName(); -} + /** @brief Apply Command Helper which does not lock */ + bool applyCommandsHelper(const std::vector>& commands); -void Environment::setState(const std::unordered_map& joints) -{ + // Command Helper function + 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); + + std::unique_ptr clone() const; + + friend class boost::serialization::access; + template + void save(Archive& ar, const unsigned int /*version*/) const // NOLINT { - std::unique_lock lock(mutex_); - state_solver_->setState(joints); - currentStateChanged(); + ar& BOOST_SERIALIZATION_NVP(resource_locator); + ar& BOOST_SERIALIZATION_NVP(commands); + ar& BOOST_SERIALIZATION_NVP(init_revision); + ar& BOOST_SERIALIZATION_NVP(current_state); + ar& boost::serialization::make_nvp("timestamp", + boost::serialization::make_binary_object(×tamp, sizeof(timestamp))); + ar& boost::serialization::make_nvp( + "current_state_timestamp", + boost::serialization::make_binary_object(¤t_state_timestamp, sizeof(current_state_timestamp))); } - std::shared_lock lock(mutex_); - triggerCurrentStateChangedCallbacks(); -} + template + void load(Archive& ar, const unsigned int /*version*/) // NOLINT + { + ar& BOOST_SERIALIZATION_NVP(resource_locator); -void Environment::setState(const std::vector& joint_names, - const Eigen::Ref& joint_values) + tesseract_environment::Commands commands; + ar& boost::serialization::make_nvp("commands", commands); + initHelper(commands); + + ar& BOOST_SERIALIZATION_NVP(init_revision); + + tesseract_scene_graph::SceneState current_state; + ar& boost::serialization::make_nvp("current_state", current_state); + setState(current_state.joints); + + ar& boost::serialization::make_nvp("timestamp", + boost::serialization::make_binary_object(×tamp, sizeof(timestamp))); + ar& boost::serialization::make_nvp( + "current_state_timestamp", + boost::serialization::make_binary_object(¤t_state_timestamp, sizeof(current_state_timestamp))); + } + + template + void serialize(Archive& ar, const unsigned int version) // NOLINT + { + boost::serialization::split_member(ar, *this, version); + } +}; + +bool Environment::Implementation::operator==(const Environment::Implementation& rhs) const { + // No need to check everything mainly the items serialized + bool equal = true; + equal &= initialized == rhs.initialized; + equal &= revision == rhs.revision; + equal &= init_revision == rhs.init_revision; + equal &= commands.size() == rhs.commands.size(); + if (!equal) + return equal; + + for (std::size_t i = 0; i < commands.size(); ++i) { - std::unique_lock lock(mutex_); - state_solver_->setState(joint_names, joint_values); - currentStateChanged(); + equal &= *(commands[i]) == *(rhs.commands[i]); + if (!equal) + return equal; } - std::shared_lock lock(mutex_); - triggerCurrentStateChangedCallbacks(); -} + equal &= current_state == rhs.current_state; + equal &= timestamp == rhs.timestamp; + equal &= current_state_timestamp == rhs.current_state_timestamp; -tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map& joints) const -{ - std::shared_lock lock(mutex_); - return state_solver_->getState(joints); -} + /** @todo uncomment after serialized */ + // equal &= is_contact_allowed_fn == rhs.is_contact_allowed_fn; + // equal &= find_tcp_cb == rhs.find_tcp_cb; -tesseract_scene_graph::SceneState Environment::getState(const std::vector& joint_names, - const Eigen::Ref& joint_values) const -{ - std::shared_lock lock(mutex_); - return state_solver_->getState(joint_names, joint_values); + return equal; } -tesseract_scene_graph::SceneState Environment::getState() const +std::unique_ptr Environment::Implementation::clone() const { - std::shared_lock lock(mutex_); - return current_state_; -} + auto cloned_env = std::make_unique(); -std::chrono::system_clock::time_point Environment::getTimestamp() const -{ - std::shared_lock lock(mutex_); - return timestamp_; -} + std::shared_lock jg_lock(joint_group_cache_mutex); + std::shared_lock kg_lock(kinematic_group_cache_mutex); + std::shared_lock jn_lock(group_joint_names_cache_mutex); + std::shared_lock discrete_lock(discrete_manager_mutex); + std::shared_lock continuous_lock(continuous_manager_mutex); -std::chrono::system_clock::time_point Environment::getCurrentStateTimestamp() const -{ - std::shared_lock lock(mutex_); - return current_state_timestamp_; -} + if (!initialized) + return cloned_env; -tesseract_scene_graph::Link::ConstPtr Environment::getLink(const std::string& name) const -{ - std::shared_lock lock(mutex_); - tesseract_scene_graph::Link::ConstPtr link = scene_graph_->getLink(name); - return link; -} + cloned_env->initialized = initialized; + cloned_env->init_revision = revision; + cloned_env->revision = revision; + cloned_env->commands = commands; + cloned_env->scene_graph = scene_graph->clone(); + cloned_env->timestamp = timestamp; + cloned_env->current_state = current_state; + cloned_env->current_state_timestamp = current_state_timestamp; -tesseract_scene_graph::JointLimits::ConstPtr Environment::getJointLimits(const std::string& joint_name) const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getJointLimits(joint_name); -} + // There is not dynamic pointer cast for std::unique_ptr + auto cloned_solver = state_solver->clone(); + auto* p = dynamic_cast(cloned_solver.get()); + if (p != nullptr) + (void)cloned_solver.release(); -bool Environment::getLinkCollisionEnabled(const std::string& name) const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getLinkCollisionEnabled(name); -} + cloned_env->state_solver = std::unique_ptr(p); + cloned_env->kinematics_information = kinematics_information; + cloned_env->kinematics_factory = kinematics_factory; + cloned_env->find_tcp_cb = find_tcp_cb; + cloned_env->collision_margin_data = collision_margin_data; -bool Environment::getLinkVisibility(const std::string& name) const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getLinkVisibility(name); -} + // Copy cache + cloned_env->joint_group_cache.reserve(joint_group_cache.size()); + for (const auto& c : joint_group_cache) + cloned_env->joint_group_cache[c.first] = (std::make_unique(*c.second)); -tesseract_common::AllowedCollisionMatrix::ConstPtr Environment::getAllowedCollisionMatrix() const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getAllowedCollisionMatrix(); + for (const auto& c : kinematic_group_cache) + cloned_env->kinematic_group_cache[c.first] = (std::make_unique(*c.second)); + + cloned_env->group_joint_names_cache = group_joint_names_cache; + + // NOLINTNEXTLINE + cloned_env->is_contact_allowed_fn = std::bind(&tesseract_scene_graph::SceneGraph::isCollisionAllowed, + cloned_env->scene_graph, + std::placeholders::_1, + std::placeholders::_2); + + if (discrete_manager) + { + cloned_env->discrete_manager = discrete_manager->clone(); + cloned_env->discrete_manager->setIsContactAllowedFn(cloned_env->is_contact_allowed_fn); + } + if (continuous_manager) + { + cloned_env->continuous_manager = continuous_manager->clone(); + cloned_env->continuous_manager->setIsContactAllowedFn(cloned_env->is_contact_allowed_fn); + } + + cloned_env->contact_managers_plugin_info = contact_managers_plugin_info; + cloned_env->contact_managers_factory = contact_managers_factory; + + return cloned_env; } -std::vector Environment::getJointNames() const +bool Environment::Implementation::initHelper(const std::vector>& commands) { - std::shared_lock lock(mutex_); - return state_solver_->getJointNames(); + if (commands.empty()) + return false; + + if (commands.at(0)->getType() != CommandType::ADD_SCENE_GRAPH) + { + CONSOLE_BRIDGE_logError("When initializing environment from command history the first command must be type " + "ADD_SCENE_GRAPH!"); + return false; + } + + clear(); + + scene_graph = std::make_shared( + std::static_pointer_cast(commands.at(0))->getSceneGraph()->getName()); + + is_contact_allowed_fn = [this](const std::string& l1, const std::string& l2) { + return scene_graph->isCollisionAllowed(l1, l2); + }; + + if (!applyCommandsHelper(commands)) + { + CONSOLE_BRIDGE_logError("When initializing environment from command history, it failed to apply a command!"); + return false; + } + + initialized = true; + init_revision = revision; + + environmentChanged(); + + return initialized; } -std::vector Environment::getActiveJointNames() const +void Environment::Implementation::setState(const std::unordered_map& joints) { - std::shared_lock lock(mutex_); - return state_solver_->getActiveJointNames(); + state_solver->setState(joints); + currentStateChanged(); } -tesseract_scene_graph::Joint::ConstPtr Environment::getJoint(const std::string& name) const +void Environment::Implementation::setState(const std::vector& joint_names, + const Eigen::Ref& joint_values) { - std::shared_lock lock(mutex_); - return scene_graph_->getJoint(name); + state_solver->setState(joint_names, joint_values); + currentStateChanged(); } -Eigen::VectorXd Environment::getCurrentJointValues() const +Eigen::VectorXd Environment::Implementation::getCurrentJointValues() const { - std::shared_lock lock(mutex_); Eigen::VectorXd jv; - std::vector active_joint_names = state_solver_->getActiveJointNames(); + std::vector active_joint_names = state_solver->getActiveJointNames(); jv.resize(static_cast(active_joint_names.size())); for (auto j = 0U; j < active_joint_names.size(); ++j) - jv(j) = current_state_.joints.at(active_joint_names[j]); + jv(j) = current_state.joints.at(active_joint_names[j]); return jv; } -Eigen::VectorXd Environment::getCurrentJointValues(const std::vector& joint_names) const +Eigen::VectorXd Environment::Implementation::getCurrentJointValues(const std::vector& joint_names) const { - std::shared_lock lock(mutex_); Eigen::VectorXd jv; jv.resize(static_cast(joint_names.size())); for (auto j = 0U; j < joint_names.size(); ++j) - jv(j) = current_state_.joints.at(joint_names[j]); + jv(j) = current_state.joints.at(joint_names[j]); return jv; } -std::string Environment::getRootLinkName() const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getRoot(); -} - -std::vector Environment::getLinkNames() const -{ - std::shared_lock lock(mutex_); - return state_solver_->getLinkNames(); -} - -std::vector Environment::getActiveLinkNames() const -{ - std::shared_lock lock(mutex_); - return state_solver_->getActiveLinkNames(); -} - -std::vector Environment::getActiveLinkNames(const std::vector& joint_names) const -{ - std::shared_lock lock(mutex_); - return scene_graph_->getJointChildrenNames(joint_names); -} - -std::vector Environment::getStaticLinkNames() const -{ - std::shared_lock lock(mutex_); - return state_solver_->getStaticLinkNames(); -} - -std::vector Environment::getStaticLinkNames(const std::vector& joint_names) const +std::vector +Environment::Implementation::getStaticLinkNames(const std::vector& joint_names) const { - std::shared_lock lock(mutex_); - std::vector active_link_names = getActiveLinkNames(joint_names); - std::vector full_link_names = state_solver_->getLinkNames(); + std::vector active_link_names = scene_graph->getJointChildrenNames(joint_names); + std::vector full_link_names = state_solver->getLinkNames(); std::vector static_link_names; static_link_names.reserve(full_link_names.size()); @@ -762,216 +598,370 @@ std::vector Environment::getStaticLinkNames(const std::vector lock(mutex_); - return state_solver_->getLinkTransforms(); -} - -Eigen::Isometry3d Environment::getLinkTransform(const std::string& link_name) const +void Environment::Implementation::clear() { - std::shared_lock lock(mutex_); - return state_solver_->getLinkTransform(link_name); -} + initialized = false; + revision = 0; + init_revision = 0; + scene_graph = nullptr; + state_solver = nullptr; + current_state = tesseract_scene_graph::SceneState(); + commands.clear(); + is_contact_allowed_fn = nullptr; + collision_margin_data = tesseract_collision::CollisionMarginData(); + kinematics_information.clear(); + contact_managers_plugin_info.clear(); + kinematics_factory = tesseract_kinematics::KinematicsPluginFactory(); + contact_managers_factory = tesseract_collision::ContactManagersPluginFactory(); -Eigen::Isometry3d Environment::getRelativeLinkTransform(const std::string& from_link_name, - const std::string& to_link_name) const -{ - std::shared_lock lock(mutex_); - return state_solver_->getRelativeLinkTransform(from_link_name, to_link_name); -} + { + std::unique_lock lock(discrete_manager_mutex); + discrete_manager = nullptr; + } -tesseract_scene_graph::StateSolver::UPtr Environment::getStateSolver() const -{ - std::shared_lock lock(mutex_); - return state_solver_->clone(); + { + std::unique_lock lock(continuous_manager_mutex); + continuous_manager = nullptr; + } + + { + std::unique_lock lock(group_joint_names_cache_mutex); + group_joint_names_cache.clear(); + } + + { + std::unique_lock lock(joint_group_cache_mutex); + joint_group_cache.clear(); + } + + { + std::unique_lock lock(kinematic_group_cache_mutex); + kinematic_group_cache.clear(); + } } -tesseract_srdf::KinematicsInformation Environment::getKinematicsInformation() const +bool Environment::Implementation::reset() { - std::shared_lock lock(mutex_); - return kinematics_information_; + Commands init_command; + if (commands.empty() || !initialized) + return false; + + init_command.reserve(static_cast(init_revision)); + for (std::size_t i = 0; i < static_cast(init_revision); ++i) + init_command.push_back(commands[i]); + + return initHelper(init_command); } -tesseract_srdf::GroupNames Environment::getGroupNames() const +void Environment::Implementation::currentStateChanged() { - std::shared_lock lock(mutex_); - return kinematics_information_.group_names; + timestamp = std::chrono::system_clock::now(); + current_state_timestamp = timestamp; + current_state = state_solver->getState(); + + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager != nullptr) + discrete_manager->setCollisionObjectsTransform(current_state.link_transforms); + + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager != nullptr) + { + std::vector active_link_names = state_solver->getActiveLinkNames(); + for (const auto& tf : current_state.link_transforms) + { + if (std::find(active_link_names.begin(), active_link_names.end(), tf.first) != active_link_names.end()) + continuous_manager->setCollisionObjectsTransform(tf.first, tf.second, tf.second); + else + continuous_manager->setCollisionObjectsTransform(tf.first, tf.second); + } + } + + { // Clear JointGroup and KinematicGroup + std::unique_lock jg_lock(joint_group_cache_mutex); + std::unique_lock kg_lock(kinematic_group_cache_mutex); + joint_group_cache.clear(); + kinematic_group_cache.clear(); + } } -tesseract_common::ContactManagersPluginInfo Environment::getContactManagersPluginInfo() const +void Environment::Implementation::environmentChanged() { - std::shared_lock lock(mutex_); - return contact_managers_plugin_info_; + timestamp = std::chrono::system_clock::now(); + std::vector active_link_names = state_solver->getActiveLinkNames(); + + { + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager != nullptr) + discrete_manager->setActiveCollisionObjects(active_link_names); + } + + { + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager != nullptr) + continuous_manager->setActiveCollisionObjects(active_link_names); + } + + { // Clear JointGroup, KinematicGroup and GroupJointNames cache + std::unique_lock jn_lock(group_joint_names_cache_mutex); + group_joint_names_cache.clear(); + } + + currentStateChanged(); } -bool Environment::setActiveDiscreteContactManager(const std::string& name) +void Environment::Implementation::triggerCurrentStateChangedCallbacks() { - std::unique_lock lock(mutex_); - std::unique_lock discrete_lock(discrete_manager_mutex_); - return setActiveDiscreteContactManagerHelper(name); + if (!event_cb.empty()) + { + SceneStateChangedEvent event(current_state); + for (const auto& cb : event_cb) + cb.second(event); + } } -tesseract_collision::DiscreteContactManager::UPtr Environment::getDiscreteContactManager(const std::string& name) const +void Environment::Implementation::triggerEnvironmentChangedCallbacks() { - std::shared_lock lock(mutex_); - tesseract_collision::DiscreteContactManager::UPtr manager = getDiscreteContactManagerHelper(name); - if (manager == nullptr) + if (!event_cb.empty()) { - CONSOLE_BRIDGE_logError("Discrete manager with %s does not exist in factory!", name.c_str()); - return nullptr; + CommandAppliedEvent event(commands, revision); + for (const auto& cb : event_cb) + cb.second(event); } - - return manager; } -bool Environment::setActiveContinuousContactManager(const std::string& name) +void Environment::Implementation::triggerCallbacks() { - std::unique_lock lock(mutex_); - std::unique_lock continous_lock(continuous_manager_mutex_); - return setActiveContinuousContactManagerHelper(name); + triggerEnvironmentChangedCallbacks(); + triggerCurrentStateChangedCallbacks(); } -tesseract_collision::DiscreteContactManager::UPtr Environment::getDiscreteContactManager() const +std::vector Environment::Implementation::getGroupJointNames(const std::string& group_name) const { - std::shared_lock lock(mutex_); - { // Clone cached manager if exists - std::shared_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_) - return discrete_manager_->clone(); + auto it = std::find(kinematics_information.group_names.begin(), kinematics_information.group_names.end(), group_name); + + if (it == kinematics_information.group_names.end()) + throw std::runtime_error("Environment, Joint group '" + group_name + "' does not exist!"); + + std::unique_lock cache_lock(group_joint_names_cache_mutex); + auto cache_it = group_joint_names_cache.find(group_name); + if (cache_it != group_joint_names_cache.end()) + { + CONSOLE_BRIDGE_logDebug("Environment, getGroupJointNames(%s) cache hit!", group_name.c_str()); + return cache_it->second; } - { // Try to create the default plugin - std::unique_lock discrete_lock(discrete_manager_mutex_); - const std::string& name = contact_managers_plugin_info_.discrete_plugin_infos.default_plugin; - discrete_manager_ = getDiscreteContactManagerHelper(name); - if (discrete_manager_ == nullptr) - { - CONSOLE_BRIDGE_logError("Discrete manager with %s does not exist in factory!", name.c_str()); - return nullptr; - } + CONSOLE_BRIDGE_logDebug("Environment, getGroupJointNames(%s) cache miss!", group_name.c_str()); + auto chain_it = kinematics_information.chain_groups.find(group_name); + if (chain_it != kinematics_information.chain_groups.end()) + { + if (chain_it->second.size() > 1) + throw std::runtime_error("Environment, Groups with multiple chains is not supported!"); + + tesseract_scene_graph::ShortestPath path = + scene_graph->getShortestPath(chain_it->second.begin()->first, chain_it->second.begin()->second); + + group_joint_names_cache[group_name] = path.active_joints; + return path.active_joints; } - return discrete_manager_->clone(); -} + auto joint_it = kinematics_information.joint_groups.find(group_name); + if (joint_it != kinematics_information.joint_groups.end()) + { + group_joint_names_cache[group_name] = joint_it->second; + return joint_it->second; + } -void Environment::clearCachedDiscreteContactManager() const -{ - std::shared_lock lock(mutex_); - std::unique_lock discrete_lock(discrete_manager_mutex_); - discrete_manager_ = nullptr; + auto link_it = kinematics_information.link_groups.find(group_name); + if (link_it != kinematics_information.link_groups.end()) + throw std::runtime_error("Environment, Link groups are currently not supported!"); + + throw std::runtime_error("Environment, failed to get group '" + group_name + "' joint names!"); } -tesseract_collision::ContinuousContactManager::UPtr Environment::getContinuousContactManager() const +std::unique_ptr +Environment::Implementation::getJointGroup(const std::string& group_name) const { - std::shared_lock lock(mutex_); - { // Clone cached manager if exists - std::shared_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_) - return continuous_manager_->clone(); + std::unique_lock cache_lock(joint_group_cache_mutex); + auto it = joint_group_cache.find(group_name); + if (it != joint_group_cache.end()) + { + CONSOLE_BRIDGE_logDebug("Environment, getJointGroup(%s) cache hit!", group_name.c_str()); + return std::make_unique(*it->second); } - { // Try to create the default plugin - std::unique_lock continuous_lock(continuous_manager_mutex_); - const std::string& name = contact_managers_plugin_info_.continuous_plugin_infos.default_plugin; - continuous_manager_ = getContinuousContactManagerHelper(name); - if (continuous_manager_ == nullptr) - { - CONSOLE_BRIDGE_logError("Continuous manager with %s does not exist in factory!", name.c_str()); - return nullptr; - } - } + CONSOLE_BRIDGE_logDebug("Environment, getJointGroup(%s) cache miss!", group_name.c_str()); + // Store copy in cache and return + std::vector joint_names = getGroupJointNames(group_name); + tesseract_kinematics::JointGroup::UPtr jg = getJointGroup(group_name, joint_names); + joint_group_cache[group_name] = std::make_unique(*jg); - return continuous_manager_->clone(); + return jg; } -void Environment::clearCachedContinuousContactManager() const +std::unique_ptr +Environment::Implementation::getJointGroup(const std::string& name, const std::vector& joint_names) const { - std::shared_lock lock(mutex_); - std::unique_lock continuous_lock(continuous_manager_mutex_); - continuous_manager_ = nullptr; + return std::make_unique(name, joint_names, *scene_graph, current_state); } -tesseract_collision::ContinuousContactManager::UPtr -Environment::getContinuousContactManager(const std::string& name) const +std::unique_ptr +Environment::Implementation::getKinematicGroup(const std::string& group_name, std::string ik_solver_name) const { - std::shared_lock lock(mutex_); - tesseract_collision::ContinuousContactManager::UPtr manager = getContinuousContactManagerHelper(name); - if (manager == nullptr) + std::unique_lock cache_lock(kinematic_group_cache_mutex); + std::pair key = std::make_pair(group_name, ik_solver_name); + auto it = kinematic_group_cache.find(key); + if (it != kinematic_group_cache.end()) { - CONSOLE_BRIDGE_logError("Continuous manager with %s does not exist in factory!", name.c_str()); + CONSOLE_BRIDGE_logDebug( + "Environment, getKinematicGroup(%s, %s) cache hit!", group_name.c_str(), ik_solver_name.c_str()); + return std::make_unique(*it->second); + } + + CONSOLE_BRIDGE_logDebug( + "Environment, getKinematicGroup(%s, %s) cache miss!", group_name.c_str(), ik_solver_name.c_str()); + std::vector joint_names = getGroupJointNames(group_name); + + if (ik_solver_name.empty()) + ik_solver_name = kinematics_factory.getDefaultInvKinPlugin(group_name); + + tesseract_kinematics::InverseKinematics::UPtr inv_kin = + kinematics_factory.createInvKin(group_name, ik_solver_name, *scene_graph, current_state); + + // TODO add error message + if (inv_kin == nullptr) return nullptr; + + // Store copy in cache and return + auto kg = std::make_unique( + group_name, joint_names, std::move(inv_kin), *scene_graph, current_state); + + kinematic_group_cache[key] = std::make_unique(*kg); + +#ifndef NDEBUG + if (!tesseract_kinematics::checkKinematics(*kg)) + { + CONSOLE_BRIDGE_logError("Check Kinematics failed. This means that inverse kinematics solution for a pose do not " + "match forward kinematics solution. Did you change the URDF recently?"); } +#endif - return manager; + return kg; } -tesseract_common::CollisionMarginData Environment::getCollisionMarginData() const +Eigen::Isometry3d Environment::Implementation::findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const { - std::shared_lock lock(mutex_); - return collision_margin_data_; + // If it is already an Isometry3d return the offset + if (manip_info.tcp_offset.index() != 0) + return std::get<1>(manip_info.tcp_offset); + + // Check if the tcp offset name is a link in the scene, if so throw an exception + const std::string& tcp_offset_name = std::get<0>(manip_info.tcp_offset); + if (state_solver->hasLinkName(tcp_offset_name)) + throw std::runtime_error("The tcp offset name '" + tcp_offset_name + + "' should not be an existing link in the scene. Assign it as the tcp_frame instead!"); + + // Check Manipulator Manager for TCP + if (kinematics_information.hasGroupTCP(manip_info.manipulator, tcp_offset_name)) + return kinematics_information.group_tcps.at(manip_info.manipulator).at(tcp_offset_name); + + // Check callbacks for TCP Offset + for (const auto& fn : find_tcp_cb) + { + try + { + Eigen::Isometry3d tcp = fn(manip_info); + return tcp; + } + catch (...) + { + CONSOLE_BRIDGE_logDebug("User Defined Find TCP Callback Failed!"); + } + } + + throw std::runtime_error("Could not find tcp by name " + tcp_offset_name + "'!"); } -std::shared_lock Environment::lockRead() const +std::unique_ptr +Environment::Implementation::getDiscreteContactManager() const { - return std::shared_lock(mutex_); + { // Clone cached manager if exists + std::shared_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager) + return discrete_manager->clone(); + } + + { // Try to create the default plugin + std::unique_lock discrete_lock(discrete_manager_mutex); + const std::string& name = contact_managers_plugin_info.discrete_plugin_infos.default_plugin; + discrete_manager = getDiscreteContactManagerHelper(name); + if (discrete_manager == nullptr) + { + CONSOLE_BRIDGE_logError("Discrete manager with %s does not exist in factory!", name.c_str()); + return nullptr; + } + } + + return discrete_manager->clone(); } -bool Environment::operator==(const Environment& rhs) const +std::unique_ptr +Environment::Implementation::getContinuousContactManager() const { - std::shared_lock lock(mutex_); - - // No need to check everything mainly the items serialized - bool equal = true; - equal &= initialized_ == rhs.initialized_; - equal &= revision_ == rhs.revision_; - equal &= init_revision_ == rhs.init_revision_; - equal &= commands_.size() == rhs.commands_.size(); - if (!equal) - return equal; + { // Clone cached manager if exists + std::shared_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager) + return continuous_manager->clone(); + } - for (std::size_t i = 0; i < commands_.size(); ++i) - { - equal &= *(commands_[i]) == *(rhs.commands_[i]); - if (!equal) - return equal; + { // Try to create the default plugin + std::unique_lock continuous_lock(continuous_manager_mutex); + const std::string& name = contact_managers_plugin_info.continuous_plugin_infos.default_plugin; + continuous_manager = getContinuousContactManagerHelper(name); + if (continuous_manager == nullptr) + { + CONSOLE_BRIDGE_logError("Continuous manager with %s does not exist in factory!", name.c_str()); + return nullptr; + } } - equal &= current_state_ == rhs.current_state_; - equal &= timestamp_ == rhs.timestamp_; - equal &= current_state_timestamp_ == rhs.current_state_timestamp_; + return continuous_manager->clone(); +} - /** @todo uncomment after serialized */ - // equal &= is_contact_allowed_fn_ == rhs.is_contact_allowed_fn_; - // equal &= find_tcp_cb_ == rhs.find_tcp_cb_; +void Environment::Implementation::clearCachedDiscreteContactManager() const +{ + std::unique_lock discrete_lock(discrete_manager_mutex); + discrete_manager = nullptr; +} - return equal; +void Environment::Implementation::clearCachedContinuousContactManager() const +{ + std::unique_lock continuous_lock(continuous_manager_mutex); + continuous_manager = nullptr; } -bool Environment::operator!=(const Environment& rhs) const { return !operator==(rhs); } -bool Environment::setActiveDiscreteContactManagerHelper(const std::string& name) +bool Environment::Implementation::setActiveDiscreteContactManagerHelper(const std::string& name) { tesseract_collision::DiscreteContactManager::UPtr manager = getDiscreteContactManagerHelper(name); if (manager == nullptr) { std::string msg = "\n Discrete manager with " + name + " does not exist in factory!\n"; msg += " Available Managers:\n"; - for (const auto& m : contact_managers_factory_.getDiscreteContactManagerPlugins()) + for (const auto& m : contact_managers_factory.getDiscreteContactManagerPlugins()) msg += " " + m.first + "\n"; CONSOLE_BRIDGE_logError(msg.c_str()); return false; } - contact_managers_plugin_info_.discrete_plugin_infos.default_plugin = name; + contact_managers_plugin_info.discrete_plugin_infos.default_plugin = name; // The calling function should be locking discrete_manager_mutex_ - discrete_manager_ = std::move(manager); + discrete_manager = std::move(manager); return true; } -bool Environment::setActiveContinuousContactManagerHelper(const std::string& name) +bool Environment::Implementation::setActiveContinuousContactManagerHelper(const std::string& name) { tesseract_collision::ContinuousContactManager::UPtr manager = getContinuousContactManagerHelper(name); @@ -979,33 +969,33 @@ bool Environment::setActiveContinuousContactManagerHelper(const std::string& nam { std::string msg = "\n Continuous manager with " + name + " does not exist in factory!\n"; msg += " Available Managers:\n"; - for (const auto& m : contact_managers_factory_.getContinuousContactManagerPlugins()) + for (const auto& m : contact_managers_factory.getContinuousContactManagerPlugins()) msg += " " + m.first + "\n"; CONSOLE_BRIDGE_logError(msg.c_str()); return false; } - contact_managers_plugin_info_.continuous_plugin_infos.default_plugin = name; + contact_managers_plugin_info.continuous_plugin_infos.default_plugin = name; // The calling function should be locking continuous_manager_mutex_ - continuous_manager_ = std::move(manager); + continuous_manager = std::move(manager); return true; } -tesseract_collision::DiscreteContactManager::UPtr -Environment::getDiscreteContactManagerHelper(const std::string& name) const +std::unique_ptr +Environment::Implementation::getDiscreteContactManagerHelper(const std::string& name) const { tesseract_collision::DiscreteContactManager::UPtr manager = - contact_managers_factory_.createDiscreteContactManager(name); + contact_managers_factory.createDiscreteContactManager(name); if (manager == nullptr) return nullptr; - manager->setIsContactAllowedFn(is_contact_allowed_fn_); - if (scene_graph_ != nullptr) + manager->setIsContactAllowedFn(is_contact_allowed_fn); + if (scene_graph != nullptr) { - for (const auto& link : scene_graph_->getLinks()) + for (const auto& link : scene_graph->getLinks()) { if (!link->collision.empty()) { @@ -1016,29 +1006,29 @@ Environment::getDiscreteContactManagerHelper(const std::string& name) const } } - manager->setActiveCollisionObjects(state_solver_->getActiveLinkNames()); + manager->setActiveCollisionObjects(state_solver->getActiveLinkNames()); } - manager->setCollisionMarginData(collision_margin_data_); + manager->setCollisionMarginData(collision_margin_data); - manager->setCollisionObjectsTransform(current_state_.link_transforms); + manager->setCollisionObjectsTransform(current_state.link_transforms); return manager; } -tesseract_collision::ContinuousContactManager::UPtr -Environment::getContinuousContactManagerHelper(const std::string& name) const +std::unique_ptr +Environment::Implementation::getContinuousContactManagerHelper(const std::string& name) const { tesseract_collision::ContinuousContactManager::UPtr manager = - contact_managers_factory_.createContinuousContactManager(name); + contact_managers_factory.createContinuousContactManager(name); if (manager == nullptr) return nullptr; - manager->setIsContactAllowedFn(is_contact_allowed_fn_); - if (scene_graph_ != nullptr) + manager->setIsContactAllowedFn(is_contact_allowed_fn); + if (scene_graph != nullptr) { - for (const auto& link : scene_graph_->getLinks()) + for (const auto& link : scene_graph->getLinks()) { if (!link->collision.empty()) { @@ -1049,13 +1039,13 @@ Environment::getContinuousContactManagerHelper(const std::string& name) const } } - manager->setActiveCollisionObjects(state_solver_->getActiveLinkNames()); + manager->setActiveCollisionObjects(state_solver->getActiveLinkNames()); } - manager->setCollisionMarginData(collision_margin_data_); + manager->setCollisionMarginData(collision_margin_data); - std::vector active_link_names = state_solver_->getActiveLinkNames(); - for (const auto& tf : current_state_.link_transforms) + std::vector active_link_names = state_solver->getActiveLinkNames(); + for (const auto& tf : current_state.link_transforms) { if (std::find(active_link_names.begin(), active_link_names.end(), tf.first) != active_link_names.end()) manager->setCollisionObjectsTransform(tf.first, tf.second, tf.second); @@ -1066,195 +1056,78 @@ Environment::getContinuousContactManagerHelper(const std::string& name) const return manager; } -void Environment::getCollisionObject(tesseract_collision::CollisionShapesConst& shapes, - tesseract_common::VectorIsometry3d& shape_poses, - const tesseract_scene_graph::Link& link) +bool Environment::Implementation::setActiveDiscreteContactManager(const std::string& name) { - for (const auto& c : link.collision) - { - shapes.push_back(c->geometry); - shape_poses.push_back(c->origin); - } + std::unique_lock discrete_lock(discrete_manager_mutex); + return setActiveDiscreteContactManagerHelper(name); } -void Environment::currentStateChanged() +std::unique_ptr +Environment::Implementation::getDiscreteContactManager(const std::string& name) const { - timestamp_ = std::chrono::system_clock::now(); - current_state_timestamp_ = timestamp_; - current_state_ = state_solver_->getState(); - - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ != nullptr) - discrete_manager_->setCollisionObjectsTransform(current_state_.link_transforms); - - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ != nullptr) + tesseract_collision::DiscreteContactManager::UPtr manager = getDiscreteContactManagerHelper(name); + if (manager == nullptr) { - std::vector active_link_names = state_solver_->getActiveLinkNames(); - for (const auto& tf : current_state_.link_transforms) - { - if (std::find(active_link_names.begin(), active_link_names.end(), tf.first) != active_link_names.end()) - continuous_manager_->setCollisionObjectsTransform(tf.first, tf.second, tf.second); - else - continuous_manager_->setCollisionObjectsTransform(tf.first, tf.second); - } + CONSOLE_BRIDGE_logError("Discrete manager with %s does not exist in factory!", name.c_str()); + return nullptr; } - { // Clear JointGroup and KinematicGroup - std::unique_lock jg_lock(joint_group_cache_mutex_); - std::unique_lock kg_lock(kinematic_group_cache_mutex_); - joint_group_cache_.clear(); - kinematic_group_cache_.clear(); - } + return manager; } -void Environment::environmentChanged() +bool Environment::Implementation::setActiveContinuousContactManager(const std::string& name) { - timestamp_ = std::chrono::system_clock::now(); - std::vector active_link_names = state_solver_->getActiveLinkNames(); - - { - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ != nullptr) - discrete_manager_->setActiveCollisionObjects(active_link_names); - } - - { - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ != nullptr) - continuous_manager_->setActiveCollisionObjects(active_link_names); - } - - { // Clear JointGroup, KinematicGroup and GroupJointNames cache - std::unique_lock jn_lock(group_joint_names_cache_mutex_); - group_joint_names_cache_.clear(); - } - - currentStateChanged(); + std::unique_lock continous_lock(continuous_manager_mutex); + return setActiveContinuousContactManagerHelper(name); } -void Environment::triggerCurrentStateChangedCallbacks() +std::unique_ptr +Environment::Implementation::getContinuousContactManager(const std::string& name) const { - if (!event_cb_.empty()) + tesseract_collision::ContinuousContactManager::UPtr manager = getContinuousContactManagerHelper(name); + if (manager == nullptr) { - SceneStateChangedEvent event(current_state_); - for (const auto& cb : event_cb_) - cb.second(event); + CONSOLE_BRIDGE_logError("Continuous manager with %s does not exist in factory!", name.c_str()); + return nullptr; } -} -void Environment::triggerEnvironmentChangedCallbacks() -{ - if (!event_cb_.empty()) - { - CommandAppliedEvent event(commands_, revision_); - for (const auto& cb : event_cb_) - cb.second(event); - } + return manager; } -bool Environment::removeLinkHelper(const std::string& name) +bool Environment::Implementation::removeLinkHelper(const std::string& name) { - if (scene_graph_->getLink(name) == nullptr) + if (scene_graph->getLink(name) == nullptr) { CONSOLE_BRIDGE_logWarn("Tried to remove link (%s) that does not exist", name.c_str()); return false; } - std::vector joints = scene_graph_->getInboundJoints(name); + std::vector joints = scene_graph->getInboundJoints(name); assert(joints.size() <= 1); // get child link names to remove - std::vector child_link_names = scene_graph_->getLinkChildrenNames(name); + std::vector child_link_names = scene_graph->getLinkChildrenNames(name); - scene_graph_->removeLink(name, true); + scene_graph->removeLink(name, true); - std::unique_lock discrete_lock(discrete_manager_mutex_); - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (discrete_manager_ != nullptr) - discrete_manager_->removeCollisionObject(name); - if (continuous_manager_ != nullptr) - continuous_manager_->removeCollisionObject(name); + std::unique_lock discrete_lock(discrete_manager_mutex); + std::unique_lock continuous_lock(continuous_manager_mutex); + if (discrete_manager != nullptr) + discrete_manager->removeCollisionObject(name); + if (continuous_manager != nullptr) + continuous_manager->removeCollisionObject(name); for (const auto& link_name : child_link_names) { - if (discrete_manager_ != nullptr) - discrete_manager_->removeCollisionObject(link_name); - if (continuous_manager_ != nullptr) - continuous_manager_->removeCollisionObject(link_name); + if (discrete_manager != nullptr) + discrete_manager->removeCollisionObject(link_name); + if (continuous_manager != nullptr) + continuous_manager->removeCollisionObject(link_name); } return true; } -Environment::UPtr Environment::clone() const -{ - auto cloned_env = std::make_unique(); - - std::shared_lock lock(mutex_); - std::shared_lock jg_lock(joint_group_cache_mutex_); - std::shared_lock kg_lock(kinematic_group_cache_mutex_); - std::shared_lock jn_lock(group_joint_names_cache_mutex_); - std::shared_lock discrete_lock(discrete_manager_mutex_); - std::shared_lock continuous_lock(continuous_manager_mutex_); - - if (!initialized_) - return cloned_env; - - cloned_env->initialized_ = initialized_; - cloned_env->init_revision_ = revision_; - cloned_env->revision_ = revision_; - cloned_env->commands_ = commands_; - cloned_env->scene_graph_ = scene_graph_->clone(); - cloned_env->timestamp_ = timestamp_; - cloned_env->current_state_ = current_state_; - cloned_env->current_state_timestamp_ = current_state_timestamp_; - - // There is not dynamic pointer cast for std::unique_ptr - auto cloned_solver = state_solver_->clone(); - auto* p = dynamic_cast(cloned_solver.get()); - if (p != nullptr) - (void)cloned_solver.release(); - - cloned_env->state_solver_ = std::unique_ptr(p); - cloned_env->kinematics_information_ = kinematics_information_; - cloned_env->kinematics_factory_ = kinematics_factory_; - cloned_env->find_tcp_cb_ = find_tcp_cb_; - cloned_env->collision_margin_data_ = collision_margin_data_; - - // Copy cache - cloned_env->joint_group_cache_.reserve(joint_group_cache_.size()); - for (const auto& c : joint_group_cache_) - cloned_env->joint_group_cache_[c.first] = (std::make_unique(*c.second)); - - for (const auto& c : kinematic_group_cache_) - cloned_env->kinematic_group_cache_[c.first] = (std::make_unique(*c.second)); - - cloned_env->group_joint_names_cache_ = group_joint_names_cache_; - - // NOLINTNEXTLINE - cloned_env->is_contact_allowed_fn_ = std::bind(&tesseract_scene_graph::SceneGraph::isCollisionAllowed, - cloned_env->scene_graph_, - std::placeholders::_1, - std::placeholders::_2); - - if (discrete_manager_) - { - cloned_env->discrete_manager_ = discrete_manager_->clone(); - cloned_env->discrete_manager_->setIsContactAllowedFn(cloned_env->is_contact_allowed_fn_); - } - if (continuous_manager_) - { - cloned_env->continuous_manager_ = continuous_manager_->clone(); - cloned_env->continuous_manager_->setIsContactAllowedFn(cloned_env->is_contact_allowed_fn_); - } - - cloned_env->contact_managers_plugin_info_ = contact_managers_plugin_info_; - cloned_env->contact_managers_factory_ = contact_managers_factory_; - - return cloned_env; -} - -bool Environment::applyCommandsHelper(const Commands& commands) +bool Environment::Implementation::applyCommandsHelper(const std::vector>& commands) { bool success = true; for (const auto& command : commands) @@ -1413,10 +1286,10 @@ bool Environment::applyCommandsHelper(const Commands& commands) } // Update the solver revision to match environment - state_solver_->setRevision(revision_); + state_solver->setRevision(revision); // If this is not true then the initHelper function has called applyCommand so do not call. - if (initialized_) + if (initialized) environmentChanged(); return success; @@ -1426,7 +1299,7 @@ bool Environment::applyCommandsHelper(const Commands& commands) //////////////// Internal Apply Command ////////////////////// ////////////////////////////////////////////////////////////// -bool Environment::applyAddCommand(const AddLinkCommand::ConstPtr& cmd) +bool Environment::Implementation::applyAddCommand(const AddLinkCommand::ConstPtr& cmd) { // The command should not allow this to occur but adding an assert to catch if something changes assert(!(!cmd->getLink() && !cmd->getJoint())); @@ -1436,13 +1309,13 @@ bool Environment::applyAddCommand(const AddLinkCommand::ConstPtr& cmd) if (!applyAddLinkCommandHelper(cmd->getLink(), cmd->getJoint(), cmd->replaceAllowed())) return false; - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand::ConstPtr& cmd) +bool Environment::Implementation::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand::ConstPtr& cmd) { const tesseract_common::JointTrajectory& traj = cmd->getTrajectory(); if (traj.empty()) @@ -1460,7 +1333,7 @@ bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand:: return false; } - const bool parent_link_exists = (scene_graph_->getLink(cmd->getParentLinkName()) != nullptr); + const bool parent_link_exists = (scene_graph->getLink(cmd->getParentLinkName()) != nullptr); if (!parent_link_exists) { CONSOLE_BRIDGE_logWarn("Tried to add trajectory link (%s) with parent link (%s) which does not exists.", @@ -1469,7 +1342,7 @@ bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand:: return false; } - auto state_solver = state_solver_->clone(); + auto state_solver_clone = state_solver->clone(); auto traj_link = std::make_shared(cmd->getLinkName()); std::vector joint_names; @@ -1495,11 +1368,11 @@ bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand:: return false; } - tesseract_scene_graph::SceneState scene_state = state_solver->getState(state.joint_names, state.position); + tesseract_scene_graph::SceneState scene_state = state_solver_clone->getState(state.joint_names, state.position); if (joint_names.empty() || !tesseract_common::isIdentical(state.joint_names, joint_names, false)) { joint_names = state.joint_names; - active_link_names = scene_graph_->getJointChildrenNames(joint_names); + active_link_names = scene_graph->getJointChildrenNames(joint_names); if (std::find(active_link_names.begin(), active_link_names.end(), cmd->getParentLinkName()) != active_link_names.end()) @@ -1514,7 +1387,7 @@ bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand:: for (const auto& link_name : active_link_names) { Eigen::Isometry3d link_transform = parent_link_tf_inv * scene_state.link_transforms[link_name]; - auto link = scene_graph_->getLink(link_name); + auto link = scene_graph->getLink(link_name); assert(link != nullptr); auto clone = link->clone(link_name + "_clone"); @@ -1542,15 +1415,16 @@ bool Environment::applyAddTrajectoryLinkCommand(const AddTrajectoryLinkCommand:: if (!applyAddLinkCommandHelper(traj_link, traj_joint, cmd->replaceAllowed())) return false; - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::ConstPtr& link, - const tesseract_scene_graph::Joint::ConstPtr& joint, - bool replace_allowed) +bool Environment::Implementation::applyAddLinkCommandHelper( + const std::shared_ptr& link, + const std::shared_ptr& joint, + bool replace_allowed) { bool link_exists = false; bool joint_exists = false; @@ -1559,13 +1433,13 @@ bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::C if (link != nullptr) { link_name = link->getName(); - link_exists = (scene_graph_->getLink(link_name) != nullptr); + link_exists = (scene_graph->getLink(link_name) != nullptr); } if (joint != nullptr) { joint_name = joint->getName(); - joint_exists = (scene_graph_->getJoint(joint_name) != nullptr); + joint_exists = (scene_graph->getJoint(joint_name) != nullptr); } if (link_exists && !replace_allowed) @@ -1602,15 +1476,15 @@ bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::C if (link_exists && !joint) { // A link is being replaced - if (!scene_graph_->addLink(*link, true)) + if (!scene_graph->addLink(*link, true)) return false; // Solver is not affected by replace links } else if (link_exists && joint_exists) { // A link and joint pair is being replaced - tesseract_scene_graph::Link::ConstPtr orig_link = scene_graph_->getLink(link_name); - tesseract_scene_graph::Joint::ConstPtr orig_joint = scene_graph_->getJoint(joint_name); + tesseract_scene_graph::Link::ConstPtr orig_link = scene_graph->getLink(link_name); + tesseract_scene_graph::Joint::ConstPtr orig_joint = scene_graph->getJoint(joint_name); if (orig_joint->child_link_name != orig_link->getName()) { @@ -1621,32 +1495,32 @@ bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::C return false; } - if (!scene_graph_->addLink(*link, true)) + if (!scene_graph->addLink(*link, true)) return false; - if (!scene_graph_->removeJoint(joint_name)) + if (!scene_graph->removeJoint(joint_name)) { // Replace with original link - if (!scene_graph_->addLink(*orig_link, true)) + if (!scene_graph->addLink(*orig_link, true)) throw std::runtime_error("Environment: Failed to replace link and joint and reset to original state."); return false; } - if (!scene_graph_->addJoint(*joint)) + if (!scene_graph->addJoint(*joint)) { // Replace with original link - if (!scene_graph_->addLink(*orig_link, true)) + if (!scene_graph->addLink(*orig_link, true)) throw std::runtime_error("Environment: Failed to replace link and joint reset to original state."); // Replace with original link - if (!scene_graph_->addJoint(*orig_joint)) + if (!scene_graph->addJoint(*orig_joint)) throw std::runtime_error("Environment: Failed to replace link and joint and reset to original state."); return false; } - if (!state_solver_->replaceJoint(*joint)) + if (!state_solver->replaceJoint(*joint)) throw std::runtime_error("Environment, failed to replace link and joint in state solver."); } else if (!link_exists && !joint) @@ -1655,33 +1529,33 @@ bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::C tesseract_scene_graph::Joint joint(joint_name); joint.type = tesseract_scene_graph::JointType::FIXED; joint.child_link_name = link_name; - joint.parent_link_name = scene_graph_->getRoot(); + joint.parent_link_name = scene_graph->getRoot(); - if (!scene_graph_->addLink(*link, joint)) + if (!scene_graph->addLink(*link, joint)) return false; - if (!state_solver_->addLink(*link, joint)) + if (!state_solver->addLink(*link, joint)) throw std::runtime_error("Environment, failed to add link and joint in state solver."); } else { // A new link and joint is being added - if (!scene_graph_->addLink(*link, *joint)) + if (!scene_graph->addLink(*link, *joint)) return false; - if (!state_solver_->addLink(*link, *joint)) + if (!state_solver->addLink(*link, *joint)) throw std::runtime_error("Environment, failed to add link and joint in state solver."); } // If Link existed remove it from collision before adding the replacing links geometry if (link_exists) { - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ != nullptr) - discrete_manager_->removeCollisionObject(link_name); + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager != nullptr) + discrete_manager->removeCollisionObject(link_name); - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ != nullptr) - continuous_manager_->removeCollisionObject(link_name); + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager != nullptr) + continuous_manager->removeCollisionObject(link_name); } // We have moved the original objects, get a pointer to them from scene_graph @@ -1691,85 +1565,85 @@ bool Environment::applyAddLinkCommandHelper(const tesseract_scene_graph::Link::C tesseract_common::VectorIsometry3d shape_poses; getCollisionObject(shapes, shape_poses, *link); - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ != nullptr) - discrete_manager_->addCollisionObject(link_name, 0, shapes, shape_poses, true); + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager != nullptr) + discrete_manager->addCollisionObject(link_name, 0, shapes, shape_poses, true); - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ != nullptr) - continuous_manager_->addCollisionObject(link_name, 0, shapes, shape_poses, true); + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager != nullptr) + continuous_manager->addCollisionObject(link_name, 0, shapes, shape_poses, true); } return true; } -bool Environment::applyMoveLinkCommand(const MoveLinkCommand::ConstPtr& cmd) +bool Environment::Implementation::applyMoveLinkCommand(const std::shared_ptr& cmd) { - if (!scene_graph_->moveLink(*cmd->getJoint())) + if (!scene_graph->moveLink(*cmd->getJoint())) return false; - if (!state_solver_->moveLink(*cmd->getJoint())) + if (!state_solver->moveLink(*cmd->getJoint())) throw std::runtime_error("Environment, failed to move link in state solver."); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyMoveJointCommand(const MoveJointCommand::ConstPtr& cmd) +bool Environment::Implementation::applyMoveJointCommand(const std::shared_ptr& cmd) { - if (!scene_graph_->moveJoint(cmd->getJointName(), cmd->getParentLink())) + if (!scene_graph->moveJoint(cmd->getJointName(), cmd->getParentLink())) return false; - if (!state_solver_->moveJoint(cmd->getJointName(), cmd->getParentLink())) + if (!state_solver->moveJoint(cmd->getJointName(), cmd->getParentLink())) throw std::runtime_error("Environment, failed to move joint in state solver."); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyRemoveLinkCommand(const RemoveLinkCommand::ConstPtr& cmd) +bool Environment::Implementation::applyRemoveLinkCommand(const std::shared_ptr& cmd) { if (!removeLinkHelper(cmd->getLinkName())) return false; - if (!state_solver_->removeLink(cmd->getLinkName())) + if (!state_solver->removeLink(cmd->getLinkName())) throw std::runtime_error("Environment, failed to remove link in state solver."); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyRemoveJointCommand(const RemoveJointCommand::ConstPtr& cmd) +bool Environment::Implementation::applyRemoveJointCommand(const std::shared_ptr& cmd) { - if (scene_graph_->getJoint(cmd->getJointName()) == nullptr) + if (scene_graph->getJoint(cmd->getJointName()) == nullptr) { CONSOLE_BRIDGE_logWarn("Tried to remove Joint (%s) that does not exist", cmd->getJointName().c_str()); return false; } - std::string target_link_name = scene_graph_->getTargetLink(cmd->getJointName())->getName(); + std::string target_link_name = scene_graph->getTargetLink(cmd->getJointName())->getName(); if (!removeLinkHelper(target_link_name)) return false; - if (!state_solver_->removeJoint(cmd->getJointName())) + if (!state_solver->removeJoint(cmd->getJointName())) throw std::runtime_error("Environment, failed to remove joint in state solver."); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& cmd) +bool Environment::Implementation::applyReplaceJointCommand(const std::shared_ptr& cmd) { - tesseract_scene_graph::Joint::ConstPtr current_joint = scene_graph_->getJoint(cmd->getJoint()->getName()); + tesseract_scene_graph::Joint::ConstPtr current_joint = scene_graph->getJoint(cmd->getJoint()->getName()); if (current_joint == nullptr) { CONSOLE_BRIDGE_logWarn("Tried to replace Joint (%s) that does not exist", cmd->getJoint()->getName().c_str()); @@ -1783,174 +1657,180 @@ bool Environment::applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& return false; } - if (!scene_graph_->removeJoint(cmd->getJoint()->getName())) + if (!scene_graph->removeJoint(cmd->getJoint()->getName())) return false; - if (!scene_graph_->addJoint(*cmd->getJoint())) + if (!scene_graph->addJoint(*cmd->getJoint())) { // Add old joint back - if (!scene_graph_->addJoint(*current_joint)) + if (!scene_graph->addJoint(*current_joint)) throw std::runtime_error("Environment: Failed to add old joint back when replace failed!"); return false; } - if (!state_solver_->replaceJoint(*cmd->getJoint())) + if (!state_solver->replaceJoint(*cmd->getJoint())) throw std::runtime_error("Environment, failed to replace joint in state solver."); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeLinkOriginCommand(const ChangeLinkOriginCommand::ConstPtr& /*cmd*/) // NOLINT +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +bool Environment::Implementation::applyChangeLinkOriginCommand( + const std::shared_ptr& /*cmd*/) { throw std::runtime_error("Unhandled environment command: CHANGE_LINK_ORIGIN"); } -bool Environment::applyChangeJointOriginCommand(const ChangeJointOriginCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeJointOriginCommand(const ChangeJointOriginCommand::ConstPtr& cmd) { - if (!scene_graph_->changeJointOrigin(cmd->getJointName(), cmd->getOrigin())) + if (!scene_graph->changeJointOrigin(cmd->getJointName(), cmd->getOrigin())) return false; - if (!state_solver_->changeJointOrigin(cmd->getJointName(), cmd->getOrigin())) + if (!state_solver->changeJointOrigin(cmd->getJointName(), cmd->getOrigin())) throw std::runtime_error("Environment, failed to change joint origin in state solver."); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisionEnabledCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeLinkCollisionEnabledCommand( + const std::shared_ptr& cmd) { - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ != nullptr) + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager != nullptr) { if (cmd->getEnabled()) - discrete_manager_->enableCollisionObject(cmd->getLinkName()); + discrete_manager->enableCollisionObject(cmd->getLinkName()); else - discrete_manager_->disableCollisionObject(cmd->getLinkName()); + discrete_manager->disableCollisionObject(cmd->getLinkName()); } - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ != nullptr) + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager != nullptr) { if (cmd->getEnabled()) - continuous_manager_->enableCollisionObject(cmd->getLinkName()); + continuous_manager->enableCollisionObject(cmd->getLinkName()); else - continuous_manager_->disableCollisionObject(cmd->getLinkName()); + continuous_manager->disableCollisionObject(cmd->getLinkName()); } - scene_graph_->setLinkCollisionEnabled(cmd->getLinkName(), cmd->getEnabled()); + scene_graph->setLinkCollisionEnabled(cmd->getLinkName(), cmd->getEnabled()); - if (scene_graph_->getLinkCollisionEnabled(cmd->getLinkName()) != cmd->getEnabled()) + if (scene_graph->getLinkCollisionEnabled(cmd->getLinkName()) != cmd->getEnabled()) return false; - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeLinkVisibilityCommand(const ChangeLinkVisibilityCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeLinkVisibilityCommand( + const std::shared_ptr& cmd) { - scene_graph_->setLinkVisibility(cmd->getLinkName(), cmd->getEnabled()); - if (scene_graph_->getLinkVisibility(cmd->getLinkName()) != cmd->getEnabled()) + scene_graph->setLinkVisibility(cmd->getLinkName(), cmd->getEnabled()); + if (scene_graph->getLinkVisibility(cmd->getLinkName()) != cmd->getEnabled()) return false; - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyModifyAllowedCollisionsCommand(const ModifyAllowedCollisionsCommand::ConstPtr& cmd) +bool Environment::Implementation::applyModifyAllowedCollisionsCommand( + const std::shared_ptr& cmd) { switch (cmd->getModifyType()) { case ModifyAllowedCollisionsType::REMOVE: { for (const auto& entry : cmd->getAllowedCollisionMatrix().getAllAllowedCollisions()) - scene_graph_->removeAllowedCollision(entry.first.first, entry.first.second); + scene_graph->removeAllowedCollision(entry.first.first, entry.first.second); break; } case ModifyAllowedCollisionsType::REPLACE: { - scene_graph_->clearAllowedCollisions(); + scene_graph->clearAllowedCollisions(); for (const auto& entry : cmd->getAllowedCollisionMatrix().getAllAllowedCollisions()) - scene_graph_->addAllowedCollision(entry.first.first, entry.first.second, entry.second); + scene_graph->addAllowedCollision(entry.first.first, entry.first.second, entry.second); break; } case ModifyAllowedCollisionsType::ADD: { for (const auto& entry : cmd->getAllowedCollisionMatrix().getAllAllowedCollisions()) - scene_graph_->addAllowedCollision(entry.first.first, entry.first.second, entry.second); + scene_graph->addAllowedCollision(entry.first.first, entry.first.second, entry.second); break; } } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyRemoveAllowedCollisionLinkCommand(const RemoveAllowedCollisionLinkCommand::ConstPtr& cmd) +bool Environment::Implementation::applyRemoveAllowedCollisionLinkCommand( + const std::shared_ptr& cmd) { - scene_graph_->removeAllowedCollision(cmd->getLinkName()); + scene_graph->removeAllowedCollision(cmd->getLinkName()); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd) +bool Environment::Implementation::applyAddSceneGraphCommand(std::shared_ptr cmd) { - if (scene_graph_->isEmpty() && cmd->getJoint()) + if (scene_graph->isEmpty() && cmd->getJoint()) return false; - std::vector pre_links = scene_graph_->getLinks(); - if (scene_graph_->isEmpty()) + std::vector pre_links = scene_graph->getLinks(); + if (scene_graph->isEmpty()) { - if (!scene_graph_->insertSceneGraph(*cmd->getSceneGraph(), cmd->getPrefix())) + if (!scene_graph->insertSceneGraph(*cmd->getSceneGraph(), cmd->getPrefix())) return false; - state_solver_ = std::make_unique(*cmd->getSceneGraph(), cmd->getPrefix()); + state_solver = std::make_unique(*cmd->getSceneGraph(), cmd->getPrefix()); } else if (!cmd->getJoint()) { // Connect root of subgraph to graph tesseract_scene_graph::Joint root_joint(cmd->getPrefix() + cmd->getSceneGraph()->getName() + "_joint"); root_joint.type = tesseract_scene_graph::JointType::FIXED; - root_joint.parent_link_name = scene_graph_->getRoot(); + root_joint.parent_link_name = scene_graph->getRoot(); root_joint.child_link_name = cmd->getPrefix() + cmd->getSceneGraph()->getRoot(); root_joint.parent_to_joint_origin_transform = Eigen::Isometry3d::Identity(); tesseract_scene_graph::SceneGraph::ConstPtr sg = cmd->getSceneGraph(); std::string prefix = cmd->getPrefix(); cmd = std::make_shared(*sg, root_joint, prefix); - if (!scene_graph_->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) + if (!scene_graph->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) return false; - if (!state_solver_->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) + if (!state_solver->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) throw std::runtime_error("Environment, failed to insert scene graph into state solver."); } else { - if (!scene_graph_->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) + if (!scene_graph->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) return false; - if (!state_solver_->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) + if (!state_solver->insertSceneGraph(*cmd->getSceneGraph(), *cmd->getJoint(), cmd->getPrefix())) throw std::runtime_error("Environment, failed to insert scene graph into state solver."); } // Now need to get list of added links to add to the contact manager - std::vector post_links = scene_graph_->getLinks(); + std::vector post_links = scene_graph->getLinks(); assert(post_links.size() > pre_links.size()); std::sort(pre_links.begin(), pre_links.end()); std::sort(post_links.begin(), post_links.end()); @@ -1961,8 +1841,8 @@ bool Environment::applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd) pre_links.end(), std::inserter(diff_links, diff_links.begin())); - std::unique_lock discrete_lock(discrete_manager_mutex_); - std::unique_lock continuous_lock(continuous_manager_mutex_); + std::unique_lock discrete_lock(discrete_manager_mutex); + std::unique_lock continuous_lock(continuous_manager_mutex); for (const auto& link : diff_links) { if (!link->collision.empty()) @@ -1971,174 +1851,179 @@ bool Environment::applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd) tesseract_common::VectorIsometry3d shape_poses; getCollisionObject(shapes, shape_poses, *link); - if (discrete_manager_ != nullptr) - discrete_manager_->addCollisionObject(link->getName(), 0, shapes, shape_poses, true); - if (continuous_manager_ != nullptr) - continuous_manager_->addCollisionObject(link->getName(), 0, shapes, shape_poses, true); + if (discrete_manager != nullptr) + discrete_manager->addCollisionObject(link->getName(), 0, shapes, shape_poses, true); + if (continuous_manager != nullptr) + continuous_manager->addCollisionObject(link->getName(), 0, shapes, shape_poses, true); } } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeJointPositionLimitsCommand(const ChangeJointPositionLimitsCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeJointPositionLimitsCommand( + const std::shared_ptr& cmd) { // First check if all of the joint exist for (const auto& jp : cmd->getLimits()) { - tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(jp.first); + tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph->getJointLimits(jp.first); if (jl == nullptr) return false; } for (const auto& jp : cmd->getLimits()) { - tesseract_scene_graph::JointLimits jl_copy = *scene_graph_->getJointLimits(jp.first); + tesseract_scene_graph::JointLimits jl_copy = *scene_graph->getJointLimits(jp.first); jl_copy.lower = jp.second.first; jl_copy.upper = jp.second.second; - if (!scene_graph_->changeJointLimits(jp.first, jl_copy)) + if (!scene_graph->changeJointLimits(jp.first, jl_copy)) return false; - if (!state_solver_->changeJointPositionLimits(jp.first, jp.second.first, jp.second.second)) + if (!state_solver->changeJointPositionLimits(jp.first, jp.second.first, jp.second.second)) throw std::runtime_error("Environment, failed to change joint position limits in state solver."); } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeJointVelocityLimitsCommand(const ChangeJointVelocityLimitsCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeJointVelocityLimitsCommand( + const std::shared_ptr& cmd) { // First check if all of the joint exist for (const auto& jp : cmd->getLimits()) { - tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(jp.first); + tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph->getJointLimits(jp.first); if (jl == nullptr) return false; } for (const auto& jp : cmd->getLimits()) { - tesseract_scene_graph::JointLimits jl_copy = *scene_graph_->getJointLimits(jp.first); + tesseract_scene_graph::JointLimits jl_copy = *scene_graph->getJointLimits(jp.first); jl_copy.velocity = jp.second; - if (!scene_graph_->changeJointLimits(jp.first, jl_copy)) + if (!scene_graph->changeJointLimits(jp.first, jl_copy)) return false; - if (!state_solver_->changeJointVelocityLimits(jp.first, jp.second)) + if (!state_solver->changeJointVelocityLimits(jp.first, jp.second)) throw std::runtime_error("Environment, failed to change joint velocity limits in state solver."); } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeJointAccelerationLimitsCommand(const ChangeJointAccelerationLimitsCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeJointAccelerationLimitsCommand( + const std::shared_ptr& cmd) { // First check if all of the joint exist for (const auto& jp : cmd->getLimits()) { - tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(jp.first); + tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph->getJointLimits(jp.first); if (jl == nullptr) return false; } for (const auto& jp : cmd->getLimits()) { - tesseract_scene_graph::JointLimits jl_copy = *scene_graph_->getJointLimits(jp.first); + tesseract_scene_graph::JointLimits jl_copy = *scene_graph->getJointLimits(jp.first); jl_copy.acceleration = jp.second; - if (!scene_graph_->changeJointLimits(jp.first, jl_copy)) + if (!scene_graph->changeJointLimits(jp.first, jl_copy)) return false; - if (!state_solver_->changeJointAccelerationLimits(jp.first, jp.second)) + if (!state_solver->changeJointAccelerationLimits(jp.first, jp.second)) throw std::runtime_error("Environment, failed to change joint acceleration limits in state solver."); } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyAddKinematicsInformationCommand(const AddKinematicsInformationCommand::ConstPtr& cmd) +bool Environment::Implementation::applyAddKinematicsInformationCommand( + const std::shared_ptr& cmd) { - kinematics_information_.insert(cmd->getKinematicsInformation()); + kinematics_information.insert(cmd->getKinematicsInformation()); if (!cmd->getKinematicsInformation().kinematics_plugin_info.empty()) { const auto& info = cmd->getKinematicsInformation().kinematics_plugin_info; for (const auto& search_path : info.search_paths) - kinematics_factory_.addSearchPath(search_path); + kinematics_factory.addSearchPath(search_path); for (const auto& search_library : info.search_libraries) - kinematics_factory_.addSearchLibrary(search_library); + kinematics_factory.addSearchLibrary(search_library); for (const auto& group : info.fwd_plugin_infos) { for (const auto& solver : group.second.plugins) - kinematics_factory_.addFwdKinPlugin(group.first, solver.first, solver.second); + kinematics_factory.addFwdKinPlugin(group.first, solver.first, solver.second); if (!group.second.default_plugin.empty()) - kinematics_factory_.setDefaultFwdKinPlugin(group.first, group.second.default_plugin); + kinematics_factory.setDefaultFwdKinPlugin(group.first, group.second.default_plugin); } for (const auto& group : info.inv_plugin_infos) { for (const auto& solver : group.second.plugins) - kinematics_factory_.addInvKinPlugin(group.first, solver.first, solver.second); + kinematics_factory.addInvKinPlugin(group.first, solver.first, solver.second); if (!group.second.default_plugin.empty()) - kinematics_factory_.setDefaultInvKinPlugin(group.first, group.second.default_plugin); + kinematics_factory.setDefaultInvKinPlugin(group.first, group.second.default_plugin); } } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyAddContactManagersPluginInfoCommand(const AddContactManagersPluginInfoCommand::ConstPtr& cmd) +bool Environment::Implementation::applyAddContactManagersPluginInfoCommand( + const std::shared_ptr& cmd) { const auto& info = cmd->getContactManagersPluginInfo(); if (!info.empty()) { - contact_managers_plugin_info_.insert(info); + contact_managers_plugin_info.insert(info); for (const auto& search_path : info.search_paths) - contact_managers_factory_.addSearchPath(search_path); + contact_managers_factory.addSearchPath(search_path); for (const auto& search_library : info.search_libraries) - contact_managers_factory_.addSearchLibrary(search_library); + contact_managers_factory.addSearchLibrary(search_library); for (const auto& cm : info.discrete_plugin_infos.plugins) - contact_managers_factory_.addDiscreteContactManagerPlugin(cm.first, cm.second); + contact_managers_factory.addDiscreteContactManagerPlugin(cm.first, cm.second); if (!info.discrete_plugin_infos.default_plugin.empty()) - contact_managers_factory_.setDefaultDiscreteContactManagerPlugin(info.discrete_plugin_infos.default_plugin); + contact_managers_factory.setDefaultDiscreteContactManagerPlugin(info.discrete_plugin_infos.default_plugin); for (const auto& cm : info.continuous_plugin_infos.plugins) - contact_managers_factory_.addContinuousContactManagerPlugin(cm.first, cm.second); + contact_managers_factory.addContinuousContactManagerPlugin(cm.first, cm.second); if (!info.continuous_plugin_infos.default_plugin.empty()) - contact_managers_factory_.setDefaultContinuousContactManagerPlugin(info.continuous_plugin_infos.default_plugin); + contact_managers_factory.setDefaultContinuousContactManagerPlugin(info.continuous_plugin_infos.default_plugin); } - if (contact_managers_factory_.hasDiscreteContactManagerPlugins()) + if (contact_managers_factory.hasDiscreteContactManagerPlugins()) { - std::string discrete_default = contact_managers_factory_.getDefaultDiscreteContactManagerPlugin(); - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ == nullptr || discrete_manager_->getName() != discrete_default) + std::string discrete_default = contact_managers_factory.getDefaultDiscreteContactManagerPlugin(); + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager == nullptr || discrete_manager->getName() != discrete_default) setActiveDiscreteContactManagerHelper(discrete_default); } else @@ -2146,11 +2031,11 @@ bool Environment::applyAddContactManagersPluginInfoCommand(const AddContactManag CONSOLE_BRIDGE_logDebug("Environment, No discrete contact manager plugins were provided"); } - if (contact_managers_factory_.hasContinuousContactManagerPlugins()) + if (contact_managers_factory.hasContinuousContactManagerPlugins()) { - std::string continuous_default = contact_managers_factory_.getDefaultContinuousContactManagerPlugin(); - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ == nullptr || continuous_manager_->getName() != continuous_default) + std::string continuous_default = contact_managers_factory.getDefaultContinuousContactManagerPlugin(); + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager == nullptr || continuous_manager->getName() != continuous_default) setActiveContinuousContactManagerHelper(continuous_default); } else @@ -2158,88 +2043,641 @@ bool Environment::applyAddContactManagersPluginInfoCommand(const AddContactManag CONSOLE_BRIDGE_logDebug("Environment, No continuous contact manager plugins were provided"); } - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applySetActiveContinuousContactManagerCommand( - const SetActiveContinuousContactManagerCommand::ConstPtr& cmd) +bool Environment::Implementation::applySetActiveContinuousContactManagerCommand( + const std::shared_ptr& cmd) { setActiveContinuousContactManagerHelper(cmd->getName()); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applySetActiveDiscreteContactManagerCommand( - const SetActiveDiscreteContactManagerCommand::ConstPtr& cmd) +bool Environment::Implementation::applySetActiveDiscreteContactManagerCommand( + const std::shared_ptr& cmd) { setActiveDiscreteContactManagerHelper(cmd->getName()); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -bool Environment::applyChangeCollisionMarginsCommand(const ChangeCollisionMarginsCommand::ConstPtr& cmd) +bool Environment::Implementation::applyChangeCollisionMarginsCommand( + const std::shared_ptr& cmd) { - collision_margin_data_.apply(cmd->getCollisionMarginData(), cmd->getCollisionMarginOverrideType()); + collision_margin_data.apply(cmd->getCollisionMarginData(), cmd->getCollisionMarginOverrideType()); - std::unique_lock continuous_lock(continuous_manager_mutex_); - if (continuous_manager_ != nullptr) - continuous_manager_->setCollisionMarginData(collision_margin_data_, CollisionMarginOverrideType::REPLACE); + std::unique_lock continuous_lock(continuous_manager_mutex); + if (continuous_manager != nullptr) + continuous_manager->setCollisionMarginData(collision_margin_data, CollisionMarginOverrideType::REPLACE); - std::unique_lock discrete_lock(discrete_manager_mutex_); - if (discrete_manager_ != nullptr) - discrete_manager_->setCollisionMarginData(collision_margin_data_, CollisionMarginOverrideType::REPLACE); + std::unique_lock discrete_lock(discrete_manager_mutex); + if (discrete_manager != nullptr) + discrete_manager->setCollisionMarginData(collision_margin_data, CollisionMarginOverrideType::REPLACE); - ++revision_; - commands_.push_back(cmd); + ++revision; + commands.push_back(cmd); return true; } -template -void Environment::save(Archive& ar, const unsigned int /*version*/) const +Environment::Environment() : impl_(std::make_unique()) {} +Environment::Environment(std::unique_ptr impl) : impl_(std::move(impl)) {} +Environment::~Environment() = default; + +bool Environment::init(const std::vector>& commands) { + bool success{ false }; + { + std::unique_lock lock(mutex_); + success = impl_->initHelper(commands); + } + + // Call the event callbacks std::shared_lock lock(mutex_); + impl_->triggerCallbacks(); - ar& BOOST_SERIALIZATION_NVP(resource_locator_); - ar& BOOST_SERIALIZATION_NVP(commands_); - ar& BOOST_SERIALIZATION_NVP(init_revision_); - ar& BOOST_SERIALIZATION_NVP(current_state_); - ar& boost::serialization::make_nvp("timestamp_", - boost::serialization::make_binary_object(×tamp_, sizeof(timestamp_))); - ar& boost::serialization::make_nvp( - "current_state_timestamp_", - boost::serialization::make_binary_object(¤t_state_timestamp_, sizeof(current_state_timestamp_))); + return success; } -template -void Environment::load(Archive& ar, const unsigned int /*version*/) +bool Environment::init(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::shared_ptr& srdf_model) { - ar& BOOST_SERIALIZATION_NVP(resource_locator_); + Commands commands = getInitCommands(scene_graph, srdf_model); + return init(commands); +} - tesseract_environment::Commands commands; - ar& boost::serialization::make_nvp("commands_", commands); - init(commands); +bool Environment::init(const std::string& urdf_string, + const std::shared_ptr& locator) +{ + { + std::unique_lock lock(mutex_); + impl_->resource_locator = locator; + } - ar& BOOST_SERIALIZATION_NVP(init_revision_); + // Parse urdf string into Scene Graph + tesseract_scene_graph::SceneGraph::Ptr scene_graph; + try + { + scene_graph = tesseract_urdf::parseURDFString(urdf_string, *locator); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("Failed to parse URDF."); + tesseract_common::printNestedException(e); + return false; + } - tesseract_scene_graph::SceneState current_state; - ar& boost::serialization::make_nvp("current_state_", current_state); - setState(current_state.joints); - - ar& boost::serialization::make_nvp("timestamp_", - boost::serialization::make_binary_object(×tamp_, sizeof(timestamp_))); - ar& boost::serialization::make_nvp( - "current_state_timestamp_", - boost::serialization::make_binary_object(¤t_state_timestamp_, sizeof(current_state_timestamp_))); + Commands commands = getInitCommands(*scene_graph); + return init(commands); +} + +bool Environment::init(const std::string& urdf_string, + const std::string& srdf_string, + const std::shared_ptr& locator) +{ + { + std::unique_lock lock(mutex_); + impl_->resource_locator = locator; + } + + // Parse urdf string into Scene Graph + tesseract_scene_graph::SceneGraph::Ptr scene_graph; + try + { + scene_graph = tesseract_urdf::parseURDFString(urdf_string, *locator); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("Failed to parse URDF."); + tesseract_common::printNestedException(e); + return false; + } + + // Parse srdf string into SRDF Model + auto srdf = std::make_shared(); + try + { + srdf->initString(*scene_graph, srdf_string, *locator); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("Failed to parse SRDF."); + tesseract_common::printNestedException(e); + return false; + } + + Commands commands = getInitCommands(*scene_graph, srdf); + return init(commands); +} + +bool Environment::init(const tesseract_common::fs::path& urdf_path, + const std::shared_ptr& locator) +{ + { + std::unique_lock lock(mutex_); + impl_->resource_locator = locator; + } + + // Parse urdf file into Scene Graph + tesseract_scene_graph::SceneGraph::Ptr scene_graph; + try + { + scene_graph = tesseract_urdf::parseURDFFile(urdf_path.string(), *locator); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("Failed to parse URDF."); + tesseract_common::printNestedException(e); + return false; + } + + Commands commands = getInitCommands(*scene_graph); + return init(commands); +} + +bool Environment::init(const tesseract_common::fs::path& urdf_path, + const tesseract_common::fs::path& srdf_path, + const std::shared_ptr& locator) +{ + { + std::unique_lock lock(mutex_); + impl_->resource_locator = locator; + } + + // Parse urdf file into Scene Graph + tesseract_scene_graph::SceneGraph::Ptr scene_graph; + try + { + scene_graph = tesseract_urdf::parseURDFFile(urdf_path.string(), *locator); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("Failed to parse URDF."); + tesseract_common::printNestedException(e); + return false; + } + + // Parse srdf file into SRDF Model + auto srdf = std::make_shared(); + try + { + srdf->initFile(*scene_graph, srdf_path.string(), *locator); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("Failed to parse SRDF."); + tesseract_common::printNestedException(e); + return false; + } + + Commands commands = getInitCommands(*scene_graph, srdf); + return init(commands); // NOLINT +} + +bool Environment::reset() +{ + bool success{ false }; + { + std::unique_lock lock(mutex_); + success = impl_->reset(); + } + + // Call the event callbacks + std::shared_lock lock(mutex_); + impl_->triggerCallbacks(); + + return success; +} + +void Environment::clear() +{ + std::unique_lock lock(mutex_); + impl_->clear(); +} + +bool Environment::isInitialized() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).initialized; +} + +int Environment::getRevision() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).revision; +} + +std::vector> Environment::getCommandHistory() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).commands; +} + +bool Environment::applyCommands(const std::vector>& commands) +{ + bool success{ false }; + { + std::unique_lock lock(mutex_); + success = impl_->applyCommandsHelper(commands); + } + std::shared_lock lock(mutex_); + impl_->triggerCallbacks(); + + return success; +} + +bool Environment::applyCommand(std::shared_ptr command) { return applyCommands({ std::move(command) }); } + +std::shared_ptr Environment::getSceneGraph() const +{ + return std::as_const(*impl_).scene_graph; +} + +std::vector Environment::getGroupJointNames(const std::string& group_name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getGroupJointNames(group_name); +} + +std::unique_ptr Environment::getJointGroup(const std::string& group_name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getJointGroup(group_name); +} + +std::unique_ptr +Environment::getJointGroup(const std::string& name, const std::vector& joint_names) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getJointGroup(name, joint_names); +} + +std::unique_ptr +Environment::getKinematicGroup(const std::string& group_name, const std::string& ik_solver_name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getKinematicGroup(group_name, ik_solver_name); +} + +// NOLINTNEXTLINE +Eigen::Isometry3d Environment::findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).findTCPOffset(manip_info); +} + +void Environment::addFindTCPOffsetCallback(const FindTCPOffsetCallbackFn& fn) +{ + std::unique_lock lock(mutex_); + impl_->find_tcp_cb.push_back(fn); +} + +std::vector Environment::getFindTCPOffsetCallbacks() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).find_tcp_cb; +} + +void Environment::addEventCallback(std::size_t hash, const EventCallbackFn& fn) +{ + std::unique_lock lock(mutex_); + impl_->event_cb[hash] = fn; +} + +void Environment::removeEventCallback(std::size_t hash) +{ + std::unique_lock lock(mutex_); + impl_->event_cb.erase(hash); +} + +void Environment::clearEventCallbacks() +{ + std::unique_lock lock(mutex_); + impl_->event_cb.clear(); +} + +std::map Environment::getEventCallbacks() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).event_cb; +} + +void Environment::setResourceLocator(std::shared_ptr locator) +{ + std::unique_lock lock(mutex_); + impl_->resource_locator = std::move(locator); +} + +std::shared_ptr Environment::getResourceLocator() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).resource_locator; +} + +void Environment::setName(const std::string& name) +{ + std::unique_lock lock(mutex_); + impl_->scene_graph->setName(name); +} + +const std::string& Environment::getName() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getName(); +} + +void Environment::setState(const std::unordered_map& joints) +{ + { + std::unique_lock lock(mutex_); + impl_->setState(joints); + } + + std::shared_lock lock(mutex_); + impl_->triggerCurrentStateChangedCallbacks(); +} + +void Environment::setState(const std::vector& joint_names, + const Eigen::Ref& joint_values) +{ + { + std::unique_lock lock(mutex_); + impl_->setState(joint_names, joint_values); + } + + std::shared_lock lock(mutex_); + impl_->triggerCurrentStateChangedCallbacks(); +} + +tesseract_scene_graph::SceneState Environment::getState(const std::unordered_map& joints) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getState(joints); +} + +tesseract_scene_graph::SceneState Environment::getState(const std::vector& joint_names, + const Eigen::Ref& joint_values) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getState(joint_names, joint_values); +} + +tesseract_scene_graph::SceneState Environment::getState() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).current_state; +} + +std::chrono::system_clock::time_point Environment::getTimestamp() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).timestamp; +} + +std::chrono::system_clock::time_point Environment::getCurrentStateTimestamp() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).current_state_timestamp; +} + +std::shared_ptr Environment::getLink(const std::string& name) const +{ + std::shared_lock lock(mutex_); + tesseract_scene_graph::Link::ConstPtr link = std::as_const(*impl_).scene_graph->getLink(name); + return link; +} + +std::shared_ptr +Environment::getJointLimits(const std::string& joint_name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getJointLimits(joint_name); +} + +bool Environment::getLinkCollisionEnabled(const std::string& name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getLinkCollisionEnabled(name); +} + +bool Environment::getLinkVisibility(const std::string& name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getLinkVisibility(name); +} + +std::shared_ptr Environment::getAllowedCollisionMatrix() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getAllowedCollisionMatrix(); +} + +std::vector Environment::getJointNames() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getJointNames(); +} + +std::vector Environment::getActiveJointNames() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getActiveJointNames(); +} + +std::shared_ptr Environment::getJoint(const std::string& name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getJoint(name); +} + +Eigen::VectorXd Environment::getCurrentJointValues() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getCurrentJointValues(); +} + +Eigen::VectorXd Environment::getCurrentJointValues(const std::vector& joint_names) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getCurrentJointValues(joint_names); +} + +std::string Environment::getRootLinkName() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getRoot(); +} + +std::vector Environment::getLinkNames() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getLinkNames(); +} + +std::vector Environment::getActiveLinkNames() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getActiveLinkNames(); +} + +std::vector Environment::getActiveLinkNames(const std::vector& joint_names) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).scene_graph->getJointChildrenNames(joint_names); +} + +std::vector Environment::getStaticLinkNames() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getStaticLinkNames(); +} + +std::vector Environment::getStaticLinkNames(const std::vector& joint_names) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getStaticLinkNames(joint_names); +} + +tesseract_common::VectorIsometry3d Environment::getLinkTransforms() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getLinkTransforms(); +} + +Eigen::Isometry3d Environment::getLinkTransform(const std::string& link_name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getLinkTransform(link_name); +} + +Eigen::Isometry3d Environment::getRelativeLinkTransform(const std::string& from_link_name, + const std::string& to_link_name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->getRelativeLinkTransform(from_link_name, to_link_name); +} + +std::unique_ptr Environment::getStateSolver() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).state_solver->clone(); +} + +tesseract_srdf::KinematicsInformation Environment::getKinematicsInformation() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).kinematics_information; +} + +std::set Environment::getGroupNames() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).kinematics_information.group_names; +} + +tesseract_common::ContactManagersPluginInfo Environment::getContactManagersPluginInfo() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).contact_managers_plugin_info; +} + +bool Environment::setActiveDiscreteContactManager(const std::string& name) +{ + std::unique_lock lock(mutex_); + return impl_->setActiveDiscreteContactManagerHelper(name); +} + +std::unique_ptr +Environment::getDiscreteContactManager(const std::string& name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getDiscreteContactManager(name); +} + +bool Environment::setActiveContinuousContactManager(const std::string& name) +{ + std::unique_lock lock(mutex_); + return impl_->setActiveContinuousContactManagerHelper(name); +} + +std::unique_ptr +Environment::getContinuousContactManager(const std::string& name) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getContinuousContactManager(name); +} + +std::unique_ptr Environment::getDiscreteContactManager() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getDiscreteContactManager(); +} + +void Environment::clearCachedDiscreteContactManager() const +{ + std::shared_lock lock(mutex_); + std::as_const(*impl_).clearCachedDiscreteContactManager(); +} + +std::unique_ptr Environment::getContinuousContactManager() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).getContinuousContactManager(); +} + +void Environment::clearCachedContinuousContactManager() const +{ + std::shared_lock lock(mutex_); + std::as_const(*impl_).clearCachedContinuousContactManager(); +} + +tesseract_common::CollisionMarginData Environment::getCollisionMarginData() const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_).collision_margin_data; +} + +std::shared_lock Environment::lockRead() const +{ + return std::shared_lock(mutex_); +} + +bool Environment::operator==(const Environment& rhs) const +{ + std::shared_lock lock(mutex_); + return std::as_const(*impl_) == std::as_const(*rhs.impl_); +} + +bool Environment::operator!=(const Environment& rhs) const { return !operator==(rhs); } + +Environment::UPtr Environment::clone() const +{ + std::shared_lock lock(mutex_); + return std::make_unique(std::as_const(*impl_).clone()); +} + +template +void Environment::save(Archive& ar, const unsigned int /*version*/) const +{ + std::shared_lock lock(mutex_); + ar& BOOST_SERIALIZATION_NVP(impl_); +} + +template +void Environment::load(Archive& ar, const unsigned int /*version*/) // NOLINT +{ + std::unique_lock lock(mutex_); + ar& BOOST_SERIALIZATION_NVP(impl_); } template diff --git a/tesseract_environment/src/environment_cache.cpp b/tesseract_environment/src/environment_cache.cpp index 201cb0c10c9..a9f064bec67 100644 --- a/tesseract_environment/src/environment_cache.cpp +++ b/tesseract_environment/src/environment_cache.cpp @@ -25,11 +25,16 @@ */ #include +#include +#include +#include namespace tesseract_environment { -DefaultEnvironmentCache::DefaultEnvironmentCache(tesseract_environment::Environment::ConstPtr env, - std::size_t cache_size) +EnvironmentCache::EnvironmentCache() = default; +EnvironmentCache::~EnvironmentCache() = default; + +DefaultEnvironmentCache::DefaultEnvironmentCache(std::shared_ptr env, std::size_t cache_size) : env_(std::move(env)), cache_size_(cache_size) { } @@ -48,14 +53,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 +71,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..432c31aaaf1 --- /dev/null +++ b/tesseract_environment/src/environment_monitor.cpp @@ -0,0 +1,55 @@ +/** + * @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 + +namespace tesseract_environment +{ +EnvironmentMonitor::EnvironmentMonitor(std::string monitor_namespace) : monitor_namespace_(std::move(monitor_namespace)) +{ +} + +EnvironmentMonitor::EnvironmentMonitor(std::shared_ptr env, std::string monitor_namespace) + : env_(std::move(env)), monitor_namespace_(std::move(monitor_namespace)) +{ +} + +EnvironmentMonitor::~EnvironmentMonitor() = default; + +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/environment_monitor_interface.cpp b/tesseract_environment/src/environment_monitor_interface.cpp new file mode 100644 index 00000000000..1e29670a852 --- /dev/null +++ b/tesseract_environment/src/environment_monitor_interface.cpp @@ -0,0 +1,32 @@ +/** + * @file environment_monitor_interface.cpp + * @brief This is a utility class for applying changes to multiple tesseract environment monitors + * + * @author Levi Armstrong + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, 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_environment +{ +EnvironmentMonitorInterface::EnvironmentMonitorInterface(std::string env_name) : env_name_(std::move(env_name)) {} +EnvironmentMonitorInterface::~EnvironmentMonitorInterface() = default; +} // namespace tesseract_environment diff --git a/tesseract_environment/src/events.cpp b/tesseract_environment/src/events.cpp new file mode 100644 index 00000000000..71011c26297 --- /dev/null +++ b/tesseract_environment/src/events.cpp @@ -0,0 +1,45 @@ +/** + * @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) {} +Event::~Event() = default; + +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..6f2701765a7 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..15c240689b5 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..58c1a6cdff6 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..e3d98cedae7 100644 --- a/tesseract_environment/test/tesseract_environment_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_unit.cpp @@ -5,19 +5,41 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #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 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..d22b826bf8d 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..ba5e07fdcc2 100644 --- a/tesseract_geometry/examples/create_geometries_example.cpp +++ b/tesseract_geometry/examples/create_geometries_example.cpp @@ -1,5 +1,6 @@ #include #include +#include #include using namespace tesseract_geometry; @@ -42,5 +43,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..0666ce5c8f6 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, @@ -62,8 +62,8 @@ class Geometry using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - explicit Geometry(GeometryType type = GeometryType::UNINITIALIZED) : type_(type) {} - virtual ~Geometry() = default; + explicit Geometry(GeometryType type = GeometryType::UNINITIALIZED); + virtual ~Geometry(); Geometry(const Geometry&) = default; Geometry& operator=(const Geometry&) = default; Geometry(Geometry&&) = default; 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..58dbb22f9e8 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/octree.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/octree.h @@ -32,72 +32,46 @@ 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) - { - } - - template - Octree(const PointT& point_cloud, - const double resolution, - const SubType sub_type, - const bool prune, - const bool binary = true) - : Geometry(GeometryType::OCTREE), sub_type_(sub_type), resolution_(resolution) - { - auto ot = std::make_shared(resolution); - - for (auto& point : point_cloud.points) - ot->updateNode(point.x, point.y, point.z, true, true); - - // Per the documentation for overload updateNode above with lazy_eval enabled this must be called after all points - // are added - ot->updateInnerOccupancy(); - if (binary) - { - ot->toMaxLikelihood(); - binary_octree_ = binary; - } - - if (prune) - { - tesseract_geometry::Octree::prune(*ot); - pruned_ = prune; - } - - octree_ = ot; - } - + Octree(std::shared_ptr octree, + OctreeSubType sub_type, + bool pruned = false, + bool binary_octree = false); Octree() = default; ~Octree() override = default; - const std::shared_ptr& getOctree() const { return octree_; } + const std::shared_ptr& getOctree() const; - SubType getSubType() const { return sub_type_; } + OctreeSubType getSubType() const; - bool getPruned() const { return pruned_; } + bool getPruned() const; + + 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 +88,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))); - - // delete children (known to be leafs at this point!) - for (unsigned int i = 0; i < 8; i++) - { - octree.deleteNodeChild(node, i); - } + static bool isNodeCollapsible(octomap::OcTree& octree, octomap::OcTreeNode* node); - 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 +127,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/octree_utils.h b/tesseract_geometry/include/tesseract_geometry/impl/octree_utils.h new file mode 100644 index 00000000000..33ef0a71023 --- /dev/null +++ b/tesseract_geometry/include/tesseract_geometry/impl/octree_utils.h @@ -0,0 +1,60 @@ +/** + * @file octree.h + * @brief Tesseract Octree Geometry Utils + * + * @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_OCTREE_UTILS_H +#define TESSERACT_GEOMETRY_OCTREE_UTILS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_geometry +{ +template +std::unique_ptr +createOctree(const PointT& point_cloud, const double resolution, const bool prune, const bool binary = true) +{ + auto ot = std::make_unique(resolution); + + for (auto& point : point_cloud.points) + ot->updateNode(point.x, point.y, point.z, true, true); + + // Per the documentation for overload updateNode above with lazy_eval enabled this must be called after all points + // are added + ot->updateInnerOccupancy(); + if (binary) + ot->toMaxLikelihood(); + + if (prune) + tesseract_geometry::Octree::prune(*ot); + + return ot; +} +} // namespace tesseract_geometry +#endif // TESSERACT_GEOMETRY_OCTREE_UTILS_H 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..91994735e9a --- /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, // NOLINT(modernize-pass-by-value) + double metallic_factor, + double roughness_factor, + const Eigen::Vector4d& emissive_factor) // NOLINT(modernize-pass-by-value) + : 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..9fe3ee2cc62 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,124 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Octree::Octree(std::shared_ptr octree, OctreeSubType sub_type, bool pruned, bool binary_octree) + : Geometry(GeometryType::OCTREE) + , octree_(std::move(octree)) + , sub_type_(sub_type) + , pruned_(pruned) + , binary_octree_(binary_octree) +{ +} + +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/geometry.cpp b/tesseract_geometry/src/geometry.cpp index 63c001dd684..3a1373bd662 100644 --- a/tesseract_geometry/src/geometry.cpp +++ b/tesseract_geometry/src/geometry.cpp @@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { +Geometry::Geometry(GeometryType type) : type_(type) {} +Geometry::~Geometry() = default; bool Geometry::operator==(const Geometry& 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..dc61c93ccad 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 @@ -33,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include using namespace tesseract_geometry; @@ -107,14 +109,16 @@ TEST(TesseractGeometrySerializeUnit, Octree) // NOLINT pc.points.emplace_back(-.5, -0.5, -0.5); pc.points.emplace_back(-.5, 0.5, 0.5); { - auto object = - std::make_shared(pc, 1, tesseract_geometry::Octree::SubType::BOX, false, true); + auto octree = tesseract_geometry::createOctree(pc, 1, false, true); + auto object = std::make_shared( + std::move(octree), 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); + auto octree = tesseract_geometry::createOctree(pc, 1, false, false); + auto object = std::make_shared( + std::move(octree), 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..e78d54212a0 100644 --- a/tesseract_geometry/test/tesseract_geometry_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_unit.cpp @@ -3,12 +3,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include TEST(TesseractGeometryUnit, Instantiation) // NOLINT { @@ -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. @@ -42,8 +42,9 @@ TEST(TesseractGeometryUnit, Instantiation) // NOLINT }; TestPointCloud pc; + auto co = tesseract_geometry::createOctree(pc, 0.01, false); auto octree_pc = - std::make_shared(pc, 0.01, tesseract_geometry::Octree::SubType::BOX, false); + std::make_shared(std::move(co), tesseract_geometry::OctreeSubType::BOX, false); } TEST(TesseractGeometryUnit, Box) // NOLINT @@ -380,14 +381,15 @@ TEST(TesseractGeometryUnit, Octree) // NOLINT }; TestPointCloud pc; - auto geom = std::make_shared(pc, 0.01, tesseract_geometry::Octree::SubType::BOX, false); + auto octree = tesseract_geometry::createOctree(pc, 0.01, false); + auto geom = std::make_shared(std::move(octree), 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..c138575c3ca 100644 --- a/tesseract_kinematics/core/CMakeLists.txt +++ b/tesseract_kinematics/core/CMakeLists.txt @@ -1,10 +1,13 @@ add_library( ${PROJECT_NAME}_core + src/forward_kinematics.cpp + src/inverse_kinematics.cpp src/rop_inv_kin.cpp src/rep_inv_kin.cpp 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..e92d12caf79 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 { @@ -54,7 +53,7 @@ class ForwardKinematics using ConstUPtr = std::unique_ptr; ForwardKinematics() = default; - virtual ~ForwardKinematics() = default; + virtual ~ForwardKinematics(); ForwardKinematics(const ForwardKinematics&) = default; ForwardKinematics& operator=(const ForwardKinematics&) = default; ForwardKinematics(ForwardKinematics&&) = default; 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..47d988cfc25 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 @@ -54,7 +53,7 @@ class InverseKinematics using ConstUPtr = std::unique_ptr; InverseKinematics() = default; - virtual ~InverseKinematics() = default; + virtual ~InverseKinematics(); InverseKinematics(const InverseKinematics&) = default; InverseKinematics& operator=(const InverseKinematics&) = default; InverseKinematics(InverseKinematics&&) = default; 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..cdafe0bd162 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 { @@ -51,7 +53,7 @@ class JointGroup using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - virtual ~JointGroup() = default; + virtual ~JointGroup(); JointGroup(const JointGroup& other); JointGroup& operator=(const JointGroup& other); JointGroup(JointGroup&&) = default; @@ -200,9 +202,11 @@ class JointGroup bool checkJoints(const Eigen::Ref& vec) const; protected: + JointGroup(); + 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..53b8e490512 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; @@ -86,7 +84,7 @@ class KinematicGroup : public JointGroup using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; - ~KinematicGroup() override = default; + ~KinematicGroup() override; KinematicGroup(const KinematicGroup& other); KinematicGroup& operator=(const KinematicGroup& other); KinematicGroup(KinematicGroup&&) = 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..32f6afe20d6 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 @@ -61,7 +62,12 @@ class InvKinFactory using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - virtual ~InvKinFactory() = default; + InvKinFactory() = default; + virtual ~InvKinFactory(); + InvKinFactory(const InvKinFactory&) = default; + InvKinFactory& operator=(const InvKinFactory&) = default; + InvKinFactory(InvKinFactory&&) = default; + InvKinFactory& operator=(InvKinFactory&&) = default; /** * @brief Create Inverse Kinematics Object @@ -71,11 +77,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; @@ -89,7 +95,12 @@ class FwdKinFactory using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - virtual ~FwdKinFactory() = default; + FwdKinFactory() = default; + virtual ~FwdKinFactory(); + FwdKinFactory(const FwdKinFactory&) = default; + FwdKinFactory& operator=(const FwdKinFactory&) = default; + FwdKinFactory(FwdKinFactory&&) = default; + FwdKinFactory& operator=(FwdKinFactory&&) = default; /** * @brief Create Inverse Kinematics Object @@ -99,11 +110,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 +257,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 +270,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 +281,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 +292,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..da142302f02 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, + double eps = 0.011, + 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/forward_kinematics.cpp b/tesseract_kinematics/core/src/forward_kinematics.cpp new file mode 100644 index 00000000000..68893bc4919 --- /dev/null +++ b/tesseract_kinematics/core/src/forward_kinematics.cpp @@ -0,0 +1,32 @@ +/** + * @file forward_kinematics.cpp + * @brief Forward kinematics 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 + +namespace tesseract_kinematics +{ +ForwardKinematics::~ForwardKinematics() = default; +} diff --git a/tesseract_kinematics/core/src/inverse_kinematics.cpp b/tesseract_kinematics/core/src/inverse_kinematics.cpp new file mode 100644 index 00000000000..52ac6e6e187 --- /dev/null +++ b/tesseract_kinematics/core/src/inverse_kinematics.cpp @@ -0,0 +1,32 @@ +/** + * @file inverse_kinematics.cpp + * @brief Inverse kinematics 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 + +namespace tesseract_kinematics +{ +InverseKinematics::~InverseKinematics() = default; +} diff --git a/tesseract_kinematics/core/src/joint_group.cpp b/tesseract_kinematics/core/src/joint_group.cpp index a4e1ba49006..8df9c956da8 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, @@ -95,6 +100,9 @@ JointGroup::JointGroup(std::string name, throw std::runtime_error("JointGroup: Static link names are not correct!"); } +JointGroup::JointGroup() = default; +JointGroup::~JointGroup() = default; + JointGroup::JointGroup(const JointGroup& other) { *this = other; } JointGroup& JointGroup::operator=(const JointGroup& other) diff --git a/tesseract_kinematics/core/src/kinematic_group.cpp b/tesseract_kinematics/core/src/kinematic_group.cpp index 175060c1a66..0fc0f1b7da3 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) @@ -96,6 +103,8 @@ KinematicGroup::KinematicGroup(std::string name, KinematicGroup::KinematicGroup(const KinematicGroup& other) : JointGroup(other) { *this = other; } +KinematicGroup::~KinematicGroup() = default; + KinematicGroup& KinematicGroup::operator=(const KinematicGroup& other) { JointGroup::operator=(other); diff --git a/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp b/tesseract_kinematics/core/src/kinematics_plugin_factory.cpp index b97342913f7..e141c4290e0 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 @@ -42,6 +46,9 @@ namespace tesseract_kinematics const std::string InvKinFactory::SECTION_NAME = "InvKin"; const std::string FwdKinFactory::SECTION_NAME = "FwdKin"; +InvKinFactory::~InvKinFactory() = default; +FwdKinFactory::~FwdKinFactory() = default; + KinematicsPluginFactory::KinematicsPluginFactory() { plugin_loader_.search_libraries_env = TESSERACT_KINEMATICS_PLUGINS_ENV; @@ -209,7 +216,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 +245,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 +273,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 +302,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..d70a5c5b8d7 --- /dev/null +++ b/tesseract_kinematics/core/src/utils.cpp @@ -0,0 +1,239 @@ +/** + * @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, double eps, 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..cf3e9847dc6 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; @@ -132,7 +133,7 @@ class SceneGraph using ConstUPtr = std::unique_ptr; SceneGraph(const std::string& name = ""); - ~SceneGraph() = default; + ~SceneGraph(); // SceneGraphs are non-copyable SceneGraph(const SceneGraph& other) = delete; SceneGraph& operator=(const SceneGraph& other) = delete; @@ -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..90fc0ce4c4d 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,19 @@ class JointDynamics using ConstPtr = std::shared_ptr; JointDynamics() = default; - JointDynamics(double damping, double friction) : damping(damping), friction(friction) {} + JointDynamics(double damping, double friction); + ~JointDynamics() = default; + + JointDynamics(const JointDynamics&) = default; + JointDynamics& operator=(const JointDynamics&) = default; + JointDynamics(JointDynamics&&) = default; + JointDynamics& operator=(JointDynamics&&) = default; + 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 +79,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 +88,13 @@ 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); + ~JointLimits() = default; + + JointLimits(const JointLimits&) = default; + JointLimits& operator=(const JointLimits&) = default; + JointLimits(JointLimits&&) = default; + JointLimits& operator=(JointLimits&&) = default; double lower{ 0 }; double upper{ 0 }; @@ -92,21 +102,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 +113,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 +123,13 @@ 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); + ~JointSafety() = default; + + JointSafety(const JointSafety&) = default; + JointSafety& operator=(const JointSafety&) = default; + JointSafety(JointSafety&&) = default; + JointSafety& operator=(JointSafety&&) = default; /// /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. @@ -169,29 +168,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 +188,31 @@ 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); + ~JointCalibration() = default; + + JointCalibration(const JointCalibration&) = default; + JointCalibration& operator=(const JointCalibration&) = default; + JointCalibration(JointCalibration&&) = default; + JointCalibration& operator=(JointCalibration&&) = default; + 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 +220,31 @@ 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); + ~JointMimic(); + + JointMimic(const JointMimic&) = default; + JointMimic& operator=(const JointMimic&) = default; + JointMimic(JointMimic&&) = default; + JointMimic& operator=(JointMimic&&) = default; + 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,9 +264,9 @@ 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; + ~Joint(); // Joints are non-copyable as their name must be unique Joint(const Joint& other) = delete; Joint& operator=(const Joint& other) = delete; @@ -294,7 +274,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 +315,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 +338,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..293f0cffe61 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,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #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 +112,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..e0e53ec64c3 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,23 @@ class Material using ConstPtr = std::shared_ptr; Material() = default; - Material(std::string name) : name_(std::move(name)) { this->clear(); } + explicit Material(std::string name); + ~Material(); - const std::string& getName() const { return name_; } + Material(const Material&) = default; + Material& operator=(const Material&) = default; + Material(Material&&) = default; + Material& operator=(Material&&) = default; + + const std::string& getName() const; + + static std::shared_ptr getDefaultMaterial(); 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; @@ -83,10 +87,6 @@ class Material void serialize(Archive& ar, const unsigned int version); // NOLINT }; -#ifndef SWIG -static const auto DEFAULT_TESSERACT_MATERIAL = std::make_shared("default_tesseract_material"); -#endif // SWIG - class Inertial { public: @@ -96,6 +96,13 @@ class Inertial using ConstPtr = std::shared_ptr; Inertial() = default; + ~Inertial() = default; + + Inertial(const Inertial&) = default; + Inertial& operator=(const Inertial&) = default; + Inertial(Inertial&&) = default; + Inertial& operator=(Inertial&&) = default; + Eigen::Isometry3d origin{ Eigen::Isometry3d::Identity() }; double mass{ 0 }; double ixx{ 0 }; @@ -105,12 +112,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 +131,22 @@ class Visual using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Visual() { this->clear(); } - Eigen::Isometry3d origin; - tesseract_geometry::Geometry::ConstPtr geometry; + Visual(); + ~Visual(); - Material::Ptr material; + Visual(const Visual&) = default; + Visual& operator=(const Visual&) = default; + Visual(Visual&&) = default; + Visual& operator=(Visual&&) = default; - void clear() - { - origin.setIdentity(); - material = DEFAULT_TESSERACT_MATERIAL; - geometry.reset(); - name.clear(); - } + Eigen::Isometry3d origin; + std::shared_ptr geometry; + Material::Ptr material; std::string name; + void clear(); + bool operator==(const Visual& rhs) const; bool operator!=(const Visual& rhs) const; @@ -161,19 +164,20 @@ class Collision using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - Collision() { this->clear(); } - Eigen::Isometry3d origin; - tesseract_geometry::Geometry::ConstPtr geometry; + Collision(); + ~Collision(); - void clear() - { - origin.setIdentity(); - geometry.reset(); - name.clear(); - } + Collision(const Collision&) = default; + Collision& operator=(const Collision&) = default; + Collision(Collision&&) = default; + Collision& operator=(Collision&&) = default; + Eigen::Isometry3d origin; + std::shared_ptr geometry; std::string name; + void clear(); + bool operator==(const Collision& rhs) const; bool operator!=(const Collision& rhs) const; @@ -189,9 +193,9 @@ 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; + ~Link(); // Links are non-copyable as their name must be unique Link(const Link& other) = delete; Link& operator=(const Link& other) = delete; @@ -199,7 +203,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 +214,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 +223,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 +238,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..a8115115ce7 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 { @@ -61,6 +61,13 @@ struct SceneState using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; + SceneState() = default; + ~SceneState(); + SceneState(const SceneState&) = default; + SceneState& operator=(const SceneState&) = default; + SceneState(SceneState&&) = default; + SceneState& operator=(SceneState&&) = default; + /** @brief The joint values used for calculating the joint and link transforms */ std::unordered_map joints; @@ -83,6 +90,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..28e4c081365 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; @@ -82,6 +198,8 @@ SceneGraph::SceneGraph(SceneGraph&& other) noexcept rebuildLinkAndJointMaps(); } +SceneGraph::~SceneGraph() = default; + SceneGraph& SceneGraph::operator=(SceneGraph&& other) noexcept { Graph::operator=(std::forward(other)); @@ -181,7 +299,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 +329,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 +338,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 +348,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 +463,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 +522,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 +582,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 +592,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 +728,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 +742,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 +768,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 +809,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 +1243,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 +1315,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..293d7944003 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,30 @@ 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)) +{ +} + +JointMimic::~JointMimic() = default; + +void JointMimic::clear() +{ + offset = 0.0; + multiplier = 1.0; + joint_name.clear(); +} + bool JointMimic::operator==(const JointMimic& rhs) const { bool equal = true; @@ -149,9 +243,68 @@ 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(); } + +Joint::~Joint() = default; + +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 +339,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..919b8aaeece 100644 --- a/tesseract_scene_graph/src/link.cpp +++ b/tesseract_scene_graph/src/link.cpp @@ -34,12 +34,30 @@ 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(); } +Material::~Material() = default; + +std::shared_ptr Material::getDefaultMaterial() +{ + static auto default_material = std::make_shared("default_tesseract_material"); + return default_material; +} + +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 +80,14 @@ 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 +120,18 @@ void Inertial::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Visual *****/ /*********************************************************/ + +Visual::Visual() { this->clear(); } +Visual::~Visual() = default; + +void Visual::clear() +{ + origin.setIdentity(); + material = Material::getDefaultMaterial(); + geometry.reset(); + name.clear(); +} + bool Visual::operator==(const Visual& rhs) const { bool equal = true; @@ -118,6 +156,16 @@ void Visual::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Collision *****/ /*********************************************************/ +Collision::Collision() { this->clear(); } +Collision::~Collision() = default; + +void Collision::clear() +{ + origin.setIdentity(); + geometry.reset(); + name.clear(); +} + bool Collision::operator==(const Collision& rhs) const { bool equal = true; @@ -140,6 +188,39 @@ void Collision::serialize(Archive& ar, const unsigned int /*version*/) /*********************************************************/ /****** Link *****/ /*********************************************************/ +Link::Link(std::string name) : name_(std::move(name)) { this->clear(); } + +Link::~Link() = default; + +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/src/scene_state.cpp b/tesseract_scene_graph/src/scene_state.cpp index 6972234ef66..bd61a31ce57 100644 --- a/tesseract_scene_graph/src/scene_state.cpp +++ b/tesseract_scene_graph/src/scene_state.cpp @@ -42,6 +42,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_scene_graph { +SceneState::~SceneState() = default; + Eigen::VectorXd SceneState::getJointValues(const std::vector& joint_names) const { Eigen::VectorXd jv; 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..5241793449b 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; @@ -66,7 +68,7 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphLinkVisualUnit) // NOLINT Visual m; EXPECT_TRUE(m.origin.isApprox(Eigen::Isometry3d::Identity())); - EXPECT_TRUE(m.material == DEFAULT_TESSERACT_MATERIAL); + EXPECT_TRUE(m.material == Material::getDefaultMaterial()); EXPECT_TRUE(m.geometry == nullptr); EXPECT_TRUE(m.name.empty()); @@ -78,7 +80,7 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphLinkVisualUnit) // NOLINT m.clear(); EXPECT_TRUE(m.origin.isApprox(Eigen::Isometry3d::Identity())); - EXPECT_TRUE(m.material == DEFAULT_TESSERACT_MATERIAL); + EXPECT_TRUE(m.material == Material::getDefaultMaterial()); EXPECT_TRUE(m.geometry == nullptr); EXPECT_TRUE(m.name.empty()); } 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..9fa47148956 100644 --- a/tesseract_srdf/CMakeLists.txt +++ b/tesseract_srdf/CMakeLists.txt @@ -87,8 +87,12 @@ configure_package( CFG_EXTRAS cmake/tesseract_srdf-extras.cmake) # 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}) +install( + FILES include/${PROJECT_NAME}/srdf_model.h + include/${PROJECT_NAME}/kinematics_information.h + 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..c6ae1af719e 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 { @@ -60,7 +57,7 @@ class SRDFModel using ConstPtr = std::shared_ptr; SRDFModel() = default; - virtual ~SRDFModel() = default; + virtual ~SRDFModel(); SRDFModel(const SRDFModel&) = default; SRDFModel& operator=(const SRDFModel&) = default; SRDFModel(SRDFModel&&) = default; @@ -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..cf6050046e6 100644 --- a/tesseract_srdf/src/srdf_model.cpp +++ b/tesseract_srdf/src/srdf_model.cpp @@ -51,9 +51,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include namespace tesseract_srdf { +SRDFModel::~SRDFModel() = default; + void SRDFModel::initFile(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& filename, const tesseract_common::ResourceLocator& locator) @@ -91,8 +96,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 +408,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/CMakeLists.txt b/tesseract_state_solver/CMakeLists.txt index ede7f1f325c..2ac8607c3e2 100644 --- a/tesseract_state_solver/CMakeLists.txt +++ b/tesseract_state_solver/CMakeLists.txt @@ -42,19 +42,20 @@ set(COVERAGE_EXCLUDE /*/bullet/BulletCollision/*) add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_library(${PROJECT_NAME}_core INTERFACE) +add_library(${PROJECT_NAME}_core src/state_solver.cpp src/mutable_state_solver.cpp) target_link_libraries( ${PROJECT_NAME}_core - INTERFACE Eigen3::Eigen - tesseract::tesseract_common - tesseract::tesseract_scene_graph - console_bridge::console_bridge) -target_compile_options(${PROJECT_NAME}_core INTERFACE ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_core INTERFACE ${TESSERACT_COMPILE_DEFINITIONS}) + PUBLIC Eigen3::Eigen + tesseract::tesseract_common + tesseract::tesseract_scene_graph + console_bridge::console_bridge) +target_compile_options(${PROJECT_NAME}_core PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) +target_compile_options(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_DEFINITIONS}) target_clang_tidy(${PROJECT_NAME}_core ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_core INTERFACE VERSION ${TESSERACT_CXX_VERSION}) -target_include_directories(${PROJECT_NAME}_core INTERFACE "$" - "$") +target_cxx_version(${PROJECT_NAME}_core PUBLIC VERSION ${TESSERACT_CXX_VERSION}) +target_include_directories(${PROJECT_NAME}_core PUBLIC "$" + "$") add_library(${PROJECT_NAME}_kdl src/kdl_state_solver.cpp) target_link_libraries( 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/mutable_state_solver.h b/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h index e5be31eafc3..dcefdedb953 100644 --- a/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h +++ b/tesseract_state_solver/include/tesseract_state_solver/mutable_state_solver.h @@ -42,7 +42,7 @@ class MutableStateSolver : public StateSolver using ConstUPtr = std::unique_ptr; MutableStateSolver() = default; - ~MutableStateSolver() override = default; + ~MutableStateSolver() override; MutableStateSolver(const MutableStateSolver&) = default; MutableStateSolver& operator=(const MutableStateSolver&) = default; MutableStateSolver(MutableStateSolver&&) = default; 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..19833424c4c 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 @@ -41,6 +41,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include namespace tesseract_scene_graph { @@ -150,15 +152,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 +190,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 +225,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 +246,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 +271,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..0462fe47439 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 { @@ -51,7 +51,7 @@ class StateSolver using ConstUPtr = std::unique_ptr; StateSolver() = default; - virtual ~StateSolver() = default; + virtual ~StateSolver(); StateSolver(const StateSolver&) = default; StateSolver& operator=(const StateSolver&) = default; StateSolver(StateSolver&&) = default; 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/mutable_state_solver.cpp b/tesseract_state_solver/src/mutable_state_solver.cpp new file mode 100644 index 00000000000..2863e9adadf --- /dev/null +++ b/tesseract_state_solver/src/mutable_state_solver.cpp @@ -0,0 +1,32 @@ +/** + * @file mutable_state_solver.cpp + * @brief Tesseract Scene Graph Mutable State Solver Interface . + * + * @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. + */ + +#include + +namespace tesseract_scene_graph +{ +MutableStateSolver::~MutableStateSolver() = default; +} 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..22e763a5798 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{ Eigen::Isometry3d::Identity() }; 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/src/state_solver.cpp b/tesseract_state_solver/src/state_solver.cpp new file mode 100644 index 00000000000..f1f590318fd --- /dev/null +++ b/tesseract_state_solver/src/state_solver.cpp @@ -0,0 +1,32 @@ +/** + * @file state_solver.cpp + * @brief Tesseract Scene Graph State Solver Interface. + * + * @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. + */ + +#include + +namespace tesseract_scene_graph +{ +StateSolver::~StateSolver() = default; +} 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..408ff256368 100644 --- a/tesseract_urdf/src/point_cloud.cpp +++ b/tesseract_urdf/src/point_cloud.cpp @@ -32,14 +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::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*/) { @@ -68,7 +71,8 @@ tesseract_geometry::Octree::Ptr tesseract_urdf::parsePointCloud(const tinyxml2:: if (cloud->points.empty()) std::throw_with_nested(std::runtime_error("PointCloud: Imported point cloud from '" + filename + "' is empty!")); - auto geom = std::make_shared(*cloud, resolution, shape_type, prune); + auto octree = tesseract_geometry::createOctree(*cloud, resolution, prune); + auto geom = std::make_shared(std::move(octree), shape_type, prune); if (geom == nullptr) std::throw_with_nested(std::runtime_error("PointCloud: Failed to create Tesseract Octree Geometry from point " "cloud!")); 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..d9034db23c7 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 @@ -67,7 +68,7 @@ tesseract_urdf::parseVisual(const tinyxml2::XMLElement* xml_element, } // get material - tesseract_scene_graph::Material::Ptr visual_material = tesseract_scene_graph::DEFAULT_TESSERACT_MATERIAL; + tesseract_scene_graph::Material::Ptr visual_material = tesseract_scene_graph::Material::getDefaultMaterial(); const tinyxml2::XMLElement* material = xml_element->FirstChildElement("material"); if (material != nullptr) { 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/fwd.h b/tesseract_visualization/include/tesseract_visualization/fwd.h new file mode 100644 index 00000000000..643d7a4ac87 --- /dev/null +++ b/tesseract_visualization/include/tesseract_visualization/fwd.h @@ -0,0 +1,12 @@ +#ifndef TESSERACT_VISUALIZATION_FWD_H +#define TESSERACT_VISUALIZATION_FWD_H + +namespace tesseract_visualization +{ +class TrajectoryInterpolator; +class TrajectoryPlayer; +class VisualizationLoader; +class Visualization; +class Marker; +} // namespace tesseract_visualization +#endif // TESSERACT_VISUALIZATION_FWD_H diff --git a/tesseract_visualization/include/tesseract_visualization/ignition/conversions.h b/tesseract_visualization/include/tesseract_visualization/ignition/conversions.h index 49ceaa0d01d..8a2beac1b58 100644 --- a/tesseract_visualization/include/tesseract_visualization/ignition/conversions.h +++ b/tesseract_visualization/include/tesseract_visualization/ignition/conversions.h @@ -32,8 +32,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include -#include +#include +#include namespace tesseract_visualization { diff --git a/tesseract_visualization/include/tesseract_visualization/ignition/tesseract_ignition_visualization.h b/tesseract_visualization/include/tesseract_visualization/ignition/tesseract_ignition_visualization.h index d532e34f452..df324289f58 100644 --- a/tesseract_visualization/include/tesseract_visualization/ignition/tesseract_ignition_visualization.h +++ b/tesseract_visualization/include/tesseract_visualization/ignition/tesseract_ignition_visualization.h @@ -62,7 +62,7 @@ class TesseractIgnitionVisualization : public tesseract_visualization::Visualiza void plotMarker(const Marker& marker, std::string ns = "") override; - void plotMarkers(const std::vector& markers, std::string ns = "") override; + void plotMarkers(const std::vector>& markers, std::string ns = "") override; void clear(std::string ns = "") override; 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.h b/tesseract_visualization/include/tesseract_visualization/visualization.h index 1f36dfef78e..71dfdbbbd66 100644 --- a/tesseract_visualization/include/tesseract_visualization/visualization.h +++ b/tesseract_visualization/include/tesseract_visualization/visualization.h @@ -26,11 +26,16 @@ #ifndef TESSERACT_VISUALIZATION_VISUALIZATION_H #define TESSERACT_VISUALIZATION_VISUALIZATION_H -#include -#include -#include -#include -#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include // clang-format off #define TESSERACT_ADD_VISUALIZATION_PLUGIN(DERIVED_CLASS, ALIAS) \ @@ -39,6 +44,8 @@ namespace tesseract_visualization { +class Marker; + /** @brief The Vizualization class */ class Visualization { @@ -98,7 +105,7 @@ class Visualization * @param markers The markers to plot * @param ns The namespace to plot the objects under */ - virtual void plotMarkers(const std::vector& markers, std::string ns = "") = 0; + virtual void plotMarkers(const std::vector>& markers, std::string ns = "") = 0; /** * @brief This is called at the start of the plotting for each iteration 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/ignition/conversions.cpp b/tesseract_visualization/src/ignition/conversions.cpp index e15f6dfe1ae..f19c9f4bf7d 100644 --- a/tesseract_visualization/src/ignition/conversions.cpp +++ b/tesseract_visualization/src/ignition/conversions.cpp @@ -41,6 +41,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_visualization { diff --git a/tesseract_visualization/src/ignition/tesseract_ignition_visualization.cpp b/tesseract_visualization/src/ignition/tesseract_ignition_visualization.cpp index c843fbad882..cba5cac3be6 100644 --- a/tesseract_visualization/src/ignition/tesseract_ignition_visualization.cpp +++ b/tesseract_visualization/src/ignition/tesseract_ignition_visualization.cpp @@ -31,9 +31,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include +#include + #include #include #include +#include #include #include #include @@ -331,7 +337,8 @@ void TesseractIgnitionVisualization::plotMarker(const Marker& marker, std::strin } } -void TesseractIgnitionVisualization::plotMarkers(const std::vector& /*markers*/, std::string /*ns*/) +void TesseractIgnitionVisualization::plotMarkers(const std::vector >& /*markers*/, + std::string /*ns*/) { ignerr << "plotMarkers is currently not implemented!" << std::endl; } 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;