Create a common config for collision checking

tesseract/tesseract_collision/include/tesseract_collision/core/types.h

@@ -325,6 +325,37 @@ struct ContactTestData
   /** @brief Indicate if search is finished */
   bool done{ false };
 };
-}  // namespace tesseract_collision
 
+/**
+ * @brief High level descriptor used in planners and utilities to specify what kind of collision check is desired.
+ *
+ * DISCRETE - Discrete contact manager using only steps specified
+ * LVS_DISCRETE - Discrete contact manager interpolating using longest valid segment
+ * CONTINUOUS - Continuous contact manager using only steps specified
+ * LVS_CONTINUOUS - Continuous contact manager interpolating using longest valid segment
+ */
+enum class CollisionEvaluatorType
+{
+  DISCRETE,
+  LVS_DISCRETE,
+  CONTINUOUS,
+  LVS_CONTINUOUS
+};
+
+/**
+ * @brief This is a high level structure containing common information that collision checking utilities need. The goal
+ * of this config is to allow all collision checking utilities and planners to use the same datastructure
+ */
+struct CollisionCheckConfig
+{
+  /** @brief Stores information about how the margins allowed between collision objects*/
+  CollisionMarginData collision_margin_data;
+  /** @brief ContactRequest that will be used for this check. Default test type: FIRST*/
+  tesseract_collision::ContactRequest contact_request{ tesseract_collision::ContactTestType::FIRST };
+  /** @brief Specifies the type of collision check to be performed. Default: DISCRETE */
+  CollisionEvaluatorType type{ CollisionEvaluatorType::DISCRETE };
+  /** @brief Longest valid segment to use if type supports lvs. Default: 0.005*/
+  double longest_valid_segment_length{ 0.005 };
+};
+}  // namespace tesseract_collision
 #endif  // TESSERACT_COLLISION_TYPES_H

tesseract/tesseract_common/include/tesseract_common/types.h

@@ -97,6 +97,5 @@ struct KinematicLimits
   Eigen::VectorXd velocity_limits;
   Eigen::VectorXd acceleration_limits;
 };
-
 }  // namespace tesseract_common
 #endif  // TESSERACT_COMMON_TYPES_H

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/trajectory_validator.h

@@ -29,6 +29,7 @@
 #include <type_traits>
 #include <tesseract_collision/core/continuous_contact_manager.h>
 #include <tesseract_collision/core/discrete_contact_manager.h>
+#include <tesseract_collision/core/types.h>
 #include <tesseract_environment/core/utils.h>
 #include <console_bridge/console.h>
 #include <trajopt/utils.hpp>
@@ -65,13 +66,8 @@ class TrajectoryValidator
   using Ptr = std::shared_ptr<TrajectoryValidator>;
 
   TrajectoryValidator(tesseract_collision::ContinuousContactManager::Ptr continuous_manager = nullptr,
-                      tesseract_collision::DiscreteContactManager::Ptr discrete_manager = nullptr,
-                      double longest_valid_segment_length = 0.01,
-                      bool verbose = false)
-    : continuous_contact_manager_(std::move(continuous_manager))
-    , discrete_contact_manager_(std::move(discrete_manager))
-    , longest_valid_segment_length_(longest_valid_segment_length)
-    , verbose_(verbose)
+                      tesseract_collision::DiscreteContactManager::Ptr discrete_manager = nullptr)
+    : continuous_contact_manager_(std::move(continuous_manager)), discrete_contact_manager_(std::move(discrete_manager))
   {
   }
 
@@ -88,8 +84,7 @@ class TrajectoryValidator
                                const PostPlanCheckType& check_type,
                                const tesseract_environment::StateSolver& state_solver,
                                const std::vector<std::string>& joint_names,
-                               const tesseract_collision::ContactRequest& request =
-                                   tesseract_collision::ContactRequest(tesseract_collision::ContactTestType::FIRST))
+                               const tesseract_collision::CollisionCheckConfig& config)
   {
     bool valid = true;
     using T = std::underlying_type<PostPlanCheckType>::type;
@@ -106,7 +101,7 @@ class TrajectoryValidator
       {
         std::vector<tesseract_collision::ContactResultMap> contacts;
         bool in_collision = tesseract_environment::checkTrajectory(
-            contacts, *discrete_contact_manager_, state_solver, joint_names, trajectory, request, verbose_);
+            contacts, *discrete_contact_manager_, state_solver, joint_names, trajectory, config);
         valid &= !in_collision;
       }
       else
@@ -124,14 +119,8 @@ class TrajectoryValidator
       if (discrete_contact_manager_)
       {
         std::vector<tesseract_collision::ContactResultMap> contacts;
-        bool in_collision = tesseract_environment::checkTrajectory(contacts,
-                                                                   *discrete_contact_manager_,
-                                                                   state_solver,
-                                                                   joint_names,
-                                                                   trajectory,
-                                                                   longest_valid_segment_length_,
-                                                                   request,
-                                                                   verbose_);
+        bool in_collision = tesseract_environment::checkTrajectory(
+            contacts, *discrete_contact_manager_, state_solver, joint_names, trajectory, config);
         valid &= !in_collision;
       }
       else
@@ -149,14 +138,8 @@ class TrajectoryValidator
       if (continuous_contact_manager_)
       {
         std::vector<tesseract_collision::ContactResultMap> contacts;
-        bool in_collision = tesseract_environment::checkTrajectory(contacts,
-                                                                   *continuous_contact_manager_,
-                                                                   state_solver,
-                                                                   joint_names,
-                                                                   trajectory,
-                                                                   longest_valid_segment_length_,
-                                                                   request,
-                                                                   verbose_);
+        bool in_collision = tesseract_environment::checkTrajectory(
+            contacts, *continuous_contact_manager_, state_solver, joint_names, trajectory, config);
         valid &= !in_collision;
       }
       else
@@ -172,9 +155,6 @@ class TrajectoryValidator
 protected:
   tesseract_collision::ContinuousContactManager::Ptr continuous_contact_manager_;
   tesseract_collision::DiscreteContactManager::Ptr discrete_contact_manager_;
-
-  double longest_valid_segment_length_;
-  bool verbose_;
 };
 
 }  // namespace tesseract_planning