Fix handling of collision margin data in environment

Author: Levi Armstrong

tesseract/tesseract_environment/include/tesseract_environment/core/environment.h

@@ -82,6 +82,7 @@ class Environment
     joint_names_.clear();
     active_link_names_.clear();
     active_joint_names_.clear();
+    collision_margin_data_ = tesseract_collision::CollisionMarginData();
 
     if (!scene_graph_->insertSceneGraph(scene_graph))
     {
@@ -641,6 +642,7 @@ class Environment
   std::vector<std::string> active_joint_names_;                   /**< A vector of active joint names */
   tesseract_collision::IsContactAllowedFn is_contact_allowed_fn_; /**< The function used to determine if two objects are
                                                                      allowed in collision */
+  tesseract_collision::CollisionMarginData collision_margin_data_;        /**< The collision margin data */
   tesseract_collision::DiscreteContactManager::Ptr discrete_manager_;     /**< The discrete contact manager object */
   tesseract_collision::ContinuousContactManager::Ptr continuous_manager_; /**< The continuous contact manager object */
   std::string discrete_manager_name_;   /**< Name of active descrete contact manager */

tesseract/tesseract_environment/src/core/environment.cpp

@@ -208,10 +208,13 @@ bool Environment::applyCommand(const Command& command)
     {
       std::lock_guard<std::mutex> lock(mutex_);
       const auto& cmd = static_cast<const tesseract_environment::ChangeDefaultContactMarginCommand&>(command);
-      tesseract_collision::CollisionMarginData collision_data = continuous_manager_->getCollisionMarginData();
-      collision_data.setDefaultCollisionMarginData(cmd.getDefaultCollisionMargin());
-      continuous_manager_->setCollisionMarginData(collision_data);
-      discrete_manager_->setCollisionMarginData(collision_data);
+      collision_margin_data_.setDefaultCollisionMarginData(cmd.getDefaultCollisionMargin());
+
+      if (continuous_manager_ != nullptr)
+        continuous_manager_->setDefaultCollisionMarginData(cmd.getDefaultCollisionMargin());
+
+      if (discrete_manager_ != nullptr)
+        discrete_manager_->setDefaultCollisionMarginData(cmd.getDefaultCollisionMargin());
 
       ++revision_;
       commands_.push_back(
@@ -225,12 +228,16 @@ bool Environment::applyCommand(const Command& command)
     {
       std::lock_guard<std::mutex> lock(mutex_);
       const auto& cmd = static_cast<const tesseract_environment::ChangePairContactMarginCommand&>(command);
-      tesseract_collision::CollisionMarginData collision_data = continuous_manager_->getCollisionMarginData();
+
       for (const auto& link_pair : cmd.getPairCollisionMarginData())
-        collision_data.setPairCollisionMarginData(link_pair.first.first, link_pair.first.second, link_pair.second);
+        collision_margin_data_.setPairCollisionMarginData(
+            link_pair.first.first, link_pair.first.second, link_pair.second);
 
-      continuous_manager_->setCollisionMarginData(collision_data);
-      discrete_manager_->setCollisionMarginData(collision_data);
+      if (continuous_manager_ != nullptr)
+        continuous_manager_->setCollisionMarginData(collision_margin_data_);
+
+      if (discrete_manager_ != nullptr)
+        discrete_manager_->setCollisionMarginData(collision_margin_data_);
 
       ++revision_;
       commands_.push_back(
@@ -891,6 +898,8 @@ Environment::getDiscreteContactManagerHelper(const std::string& name) const
     manager->setActiveCollisionObjects(active_link_names_);
   }
 
+  manager->setCollisionMarginData(collision_margin_data_);
+
   return manager;
 }
 
@@ -919,6 +928,8 @@ Environment::getContinuousContactManagerHelper(const std::string& name) const
     manager->setActiveCollisionObjects(active_link_names_);
   }
 
+  manager->setCollisionMarginData(collision_margin_data_);
+
   return manager;
 }