Fix tesseract_environment assignment of allowed collision function

tesseract-robotics · Oct 29, 2021 · 8445d50 · 8445d50
1 parent a8a19f7
commit 8445d50
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp
@@ -54,16 +54,16 @@ bool Environment::initHelper(const Commands& commands)
       std::static_pointer_cast<const AddSceneGraphCommand>(commands.at(0))->getSceneGraph()->getName());
   scene_graph_const_ = scene_graph_;
 
+  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;
   }
 
-  is_contact_allowed_fn_ = [this](const std::string& l1, const std::string& l2) {
-    return scene_graph_->isCollisionAllowed(l1, l2);
-  };
-
   initialized_ = true;
   init_revision_ = revision_;
 

diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp
@@ -265,6 +265,10 @@ Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitT
   EXPECT_EQ(group_names_ki[0], "manipulator");
   EXPECT_EQ(group_names_ki[1], "manipulator_joint_group");
 
+  // Check allowed collision matrix is not nullptr
+  EXPECT_TRUE(env->getDiscreteContactManager()->getIsContactAllowedFn() != nullptr);
+  EXPECT_TRUE(env->getContinuousContactManager()->getIsContactAllowedFn() != nullptr);
+
   // Get active contact managers
   {
     tesseract_common::ContactManagersPluginInfo cm_info = env->getContactManagersPluginInfo();