Update because of changes with collision margin data

Levi-Armstrong · Levi-Armstrong · commit d661f5cd46df · 2025-04-10T22:21:50.000-05:00
diff --git a/tesseract_monitoring/src/contact_monitor.cpp b/tesseract_monitoring/src/contact_monitor.cpp
@@ -103,7 +103,7 @@ struct ContactMonitor::Implementation
       throw std::runtime_error("Contact monitor failed to get discrete contact manager from environment!");
 
     manager->setActiveCollisionObjects(monitored_link_names);
-    manager->setDefaultCollisionMarginData(contact_distance);
+    manager->setDefaultCollisionMargin(contact_distance);
     for (const auto& disabled_link_name : disabled_link_names)
       manager->disableCollisionObject(disabled_link_name);
 
diff --git a/tesseract_msgs/CMakeLists.txt b/tesseract_msgs/CMakeLists.txt
@@ -41,7 +41,7 @@ add_message_files(
     ChainGroup.msg
     CollisionGeometry.msg
     CollisionMarginData.msg
-    CollisionMarginOverrideType.msg
+    CollisionMarginPairOverrideType.msg
     ContactManagersPluginInfo.msg
     ContactMarginPair.msg
     ContactResult.msg
diff --git a/tesseract_msgs/msg/CollisionMarginOverrideType.msg b/tesseract_msgs/msg/CollisionMarginOverrideType.msg
diff --git a/tesseract_msgs/msg/CollisionMarginPairOverrideType.msg b/tesseract_msgs/msg/CollisionMarginPairOverrideType.msg
@@ -0,0 +1,12 @@
+# Possible Override Types
+# Do not apply contact margin data
+uint8 NONE=0
+# Replace the contact manager's CollisionMarginData
+uint8 REPLACE=1
+# Modify the contact managers default margin and pair margin.
+# This will preserve existing pairs not being modified by the provided margin data.
+# If a pair already exist it will be updated with the provided margin data.
+uint8 MODIFY=2
+
+# one of the option above
+uint8 type
diff --git a/tesseract_msgs/msg/EnvironmentCommand.msg b/tesseract_msgs/msg/EnvironmentCommand.msg
@@ -92,8 +92,10 @@ tesseract_msgs/StringDoublePair[] change_joint_acceleration_limits
 tesseract_msgs/KinematicsInformation add_kinematics_information
 
 # CHANGE_COLLISION_MARGINS Command
-tesseract_msgs/CollisionMarginData collision_margin_data
-tesseract_msgs/CollisionMarginOverrideType collision_margin_override_type
+bool has_collision_default_margin
+float64 collision_default_margin
+tesseract_msgs/ContactMarginPair[] collision_margin_pair_data
+tesseract_msgs/CollisionMarginPairOverrideType collision_margin_pair_override_type
 
 # ADD_CONTACT_MANAGERS_PLUGIN_INFO Command
 tesseract_msgs/ContactManagersPluginInfo add_contact_managers_plugin_info
diff --git a/tesseract_rosutils/include/tesseract_rosutils/utils.h b/tesseract_rosutils/include/tesseract_rosutils/utils.h
@@ -56,7 +56,7 @@ ROS_DECLARE_MESSAGE(ChainGroup)
 ROS_DECLARE_MESSAGE(CollisionGeometry)
 ROS_DECLARE_MESSAGE(ContactMarginPair)
 ROS_DECLARE_MESSAGE(CollisionMarginData)
-ROS_DECLARE_MESSAGE(CollisionMarginOverrideType)
+ROS_DECLARE_MESSAGE(CollisionMarginPairOverrideType)
 ROS_DECLARE_MESSAGE(ContactResult)
 ROS_DECLARE_MESSAGE(ContactResultVector)
 ROS_DECLARE_MESSAGE(ContactManagersPluginInfo)
@@ -212,10 +212,10 @@ toMsg(const tesseract_common::PairsCollisionMarginData& contact_margin_pairs);
 tesseract_common::CollisionMarginData fromMsg(const tesseract_msgs::CollisionMarginData& contact_margin_data_msg);
 tesseract_msgs::CollisionMarginData toMsg(const tesseract_common::CollisionMarginData& contact_margin_data);
 
-tesseract_common::CollisionMarginOverrideType
-fromMsg(const tesseract_msgs::CollisionMarginOverrideType& contact_margin_override_type_msg);
-tesseract_msgs::CollisionMarginOverrideType
-toMsg(const tesseract_common::CollisionMarginOverrideType& contact_margin_override_type);
+tesseract_common::CollisionMarginPairOverrideType
+fromMsg(const tesseract_msgs::CollisionMarginPairOverrideType& contact_margin_pair_override_type_msg);
+tesseract_msgs::CollisionMarginPairOverrideType
+toMsg(const tesseract_common::CollisionMarginPairOverrideType& contact_margin_pair_override_type);
 
 tesseract_scene_graph::Joint fromMsg(const tesseract_msgs::Joint& joint_msg);
 
diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp
@@ -203,7 +203,7 @@ getContactResultsMarkerArrayMsg(int& id_counter,
     if (marker.margin_fn != nullptr)
       safety_distance = marker.margin_fn(dist.link_names[0], dist.link_names[1]);
     else
-      safety_distance = marker.margin_data.getPairCollisionMargin(dist.link_names[0], dist.link_names[1]);
+      safety_distance = marker.margin_data.getCollisionMargin(dist.link_names[0], dist.link_names[1]);
 
     auto base_material = std::make_shared<tesseract_scene_graph::Material>("base_material");
     if (dist.distance < 0)
diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp
@@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tesseract_msgs/ChainGroup.h>
 #include <tesseract_msgs/CollisionGeometry.h>
 #include <tesseract_msgs/CollisionMarginData.h>
-#include <tesseract_msgs/CollisionMarginOverrideType.h>
+#include <tesseract_msgs/CollisionMarginPairOverrideType.h>
 #include <tesseract_msgs/ContactMarginPair.h>
 #include <tesseract_msgs/ContactResultVector.h>
 #include <tesseract_msgs/ContactManagersPluginInfo.h>
@@ -1028,15 +1028,15 @@ toMsg(const tesseract_common::PairsCollisionMarginData& contact_margin_pairs)
 
 tesseract_common::CollisionMarginData fromMsg(const tesseract_msgs::CollisionMarginData& contact_margin_data_msg)
 {
-  tesseract_common::PairsCollisionMarginData contact_margin_pairs = fromMsg(contact_margin_data_msg.margin_pairs);
-  return tesseract_common::CollisionMarginData(contact_margin_data_msg.default_margin, contact_margin_pairs);
+  tesseract_common::CollisionMarginPairData pair_data(fromMsg(contact_margin_data_msg.margin_pairs));
+  return tesseract_common::CollisionMarginData(contact_margin_data_msg.default_margin, pair_data);
 }
 
 tesseract_msgs::CollisionMarginData toMsg(const tesseract_common::CollisionMarginData& contact_margin_data)
 {
   tesseract_msgs::CollisionMarginData contact_margin_data_msg;
   contact_margin_data_msg.default_margin = contact_margin_data.getDefaultCollisionMargin();
-  for (const auto& pair : contact_margin_data.getPairCollisionMargins())
+  for (const auto& pair : contact_margin_data.getCollisionMarginPairData().getCollisionMargins())
   {
     tesseract_msgs::ContactMarginPair cmp;
     cmp.first.first = pair.first.first;
@@ -1047,80 +1047,53 @@ tesseract_msgs::CollisionMarginData toMsg(const tesseract_common::CollisionMargi
   return contact_margin_data_msg;
 }
 
-tesseract_common::CollisionMarginOverrideType
-fromMsg(const tesseract_msgs::CollisionMarginOverrideType& contact_margin_override_type_msg)
+tesseract_common::CollisionMarginPairOverrideType
+fromMsg(const tesseract_msgs::CollisionMarginPairOverrideType& contact_margin_pair_override_type_msg)
 {
-  switch (contact_margin_override_type_msg.type)
+  switch (contact_margin_pair_override_type_msg.type)
   {
-    case tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN:
+    case tesseract_msgs::CollisionMarginPairOverrideType::MODIFY:
     {
-      return tesseract_common::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN;
+      return tesseract_common::CollisionMarginPairOverrideType::MODIFY;
     }
-    case tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN:
+    case tesseract_msgs::CollisionMarginPairOverrideType::REPLACE:
     {
-      return tesseract_common::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN;
+      return tesseract_common::CollisionMarginPairOverrideType::REPLACE;
     }
-    case tesseract_msgs::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN:
+    case tesseract_msgs::CollisionMarginPairOverrideType::NONE:
     {
-      return tesseract_common::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN;
-    }
-    case tesseract_msgs::CollisionMarginOverrideType::MODIFY:
-    {
-      return tesseract_common::CollisionMarginOverrideType::MODIFY;
-    }
-    case tesseract_msgs::CollisionMarginOverrideType::REPLACE:
-    {
-      return tesseract_common::CollisionMarginOverrideType::REPLACE;
-    }
-    case tesseract_msgs::CollisionMarginOverrideType::NONE:
-    {
-      return tesseract_common::CollisionMarginOverrideType::NONE;
+      return tesseract_common::CollisionMarginPairOverrideType::NONE;
     }
     default:
     {
-      throw std::runtime_error("fromMsg: Invalid CollisionMarginOverrideType!");
+      throw std::runtime_error("fromMsg: Invalid CollisionMarginPairOverrideType!");
     }
   }
 }
 
-tesseract_msgs::CollisionMarginOverrideType
-toMsg(const tesseract_common::CollisionMarginOverrideType& contact_margin_override_type)
+tesseract_msgs::CollisionMarginPairOverrideType
+toMsg(const tesseract_common::CollisionMarginPairOverrideType& contact_margin_pair_override_type)
 {
-  tesseract_msgs::CollisionMarginOverrideType contact_margin_override_type_msg;
-  switch (static_cast<int>(contact_margin_override_type))
+  tesseract_msgs::CollisionMarginPairOverrideType contact_margin_pair_override_type_msg;
+  switch (static_cast<int>(contact_margin_pair_override_type))
   {
-    case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN):
+    case static_cast<int>(tesseract_collision::CollisionMarginPairOverrideType::MODIFY):
     {
-      contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN;
+      contact_margin_pair_override_type_msg.type = tesseract_msgs::CollisionMarginPairOverrideType::MODIFY;
       break;
     }
-    case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN):
+    case static_cast<int>(tesseract_collision::CollisionMarginPairOverrideType::REPLACE):
     {
-      contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN;
+      contact_margin_pair_override_type_msg.type = tesseract_msgs::CollisionMarginPairOverrideType::REPLACE;
       break;
     }
-    case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN):
+    case static_cast<int>(tesseract_collision::CollisionMarginPairOverrideType::NONE):
     {
-      contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN;
-      break;
-    }
-    case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::MODIFY):
-    {
-      contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::MODIFY;
-      break;
-    }
-    case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::REPLACE):
-    {
-      contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::REPLACE;
-      break;
-    }
-    case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::NONE):
-    {
-      contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::NONE;
+      contact_margin_pair_override_type_msg.type = tesseract_msgs::CollisionMarginPairOverrideType::NONE;
       break;
     }
   }
-  return contact_margin_override_type_msg;
+  return contact_margin_pair_override_type_msg;
 }
 
 bool toMsg(std::vector<tesseract_msgs::AllowedCollisionEntry>& acm_msg,
@@ -1350,8 +1323,14 @@ bool toMsg(tesseract_msgs::EnvironmentCommand& command_msg, const tesseract_envi
     {
       command_msg.command = tesseract_msgs::EnvironmentCommand::CHANGE_COLLISION_MARGINS;
       const auto& cmd = static_cast<const tesseract_environment::ChangeCollisionMarginsCommand&>(command);
-      command_msg.collision_margin_data = toMsg(cmd.getCollisionMarginData());
-      command_msg.collision_margin_override_type = toMsg(cmd.getCollisionMarginOverrideType());
+
+      std::optional<double> default_margin = cmd.getDefaultCollisionMargin();
+      command_msg.has_collision_default_margin = default_margin.has_value();
+      if (default_margin.has_value())
+        command_msg.collision_default_margin = default_margin.value();
+
+      command_msg.collision_margin_pair_data = toMsg(cmd.getCollisionMarginPairData().getCollisionMargins());
+      command_msg.collision_margin_pair_override_type = toMsg(cmd.getCollisionMarginPairOverrideType());
       return true;
     }
     case tesseract_environment::CommandType::ADD_CONTACT_MANAGERS_PLUGIN_INFO:
@@ -1538,10 +1517,18 @@ std::shared_ptr<tesseract_environment::Command> fromMsg(const tesseract_msgs::En
     }
     case tesseract_msgs::EnvironmentCommand::CHANGE_COLLISION_MARGINS:
     {
-      tesseract_common::CollisionMarginData collision_margin_data = fromMsg(command_msg.collision_margin_data);
-      tesseract_common::CollisionMarginOverrideType override_type = fromMsg(command_msg.collision_margin_override_type);
-      return std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(collision_margin_data,
-                                                                                    override_type);
+      tesseract_common::CollisionMarginPairData pair_margin_data(fromMsg(command_msg.collision_margin_pair_data));
+      tesseract_common::CollisionMarginPairOverrideType pair_override_type =
+          fromMsg(command_msg.collision_margin_pair_override_type);
+
+      if (command_msg.has_collision_default_margin)
+      {
+        return std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(
+            command_msg.collision_default_margin, pair_margin_data, pair_override_type);
+      }
+
+      return std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(pair_margin_data,
+                                                                                    pair_override_type);
     }
     case tesseract_msgs::EnvironmentCommand::ADD_CONTACT_MANAGERS_PLUGIN_INFO:
     {

Original file line number	Diff line number	Diff line change
`@@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH`
`33`	`33`	`#include <tesseract_msgs/ChainGroup.h>`
`34`	`34`	`#include <tesseract_msgs/CollisionGeometry.h>`
`35`	`35`	`#include <tesseract_msgs/CollisionMarginData.h>`
`36`		`-#include <tesseract_msgs/CollisionMarginOverrideType.h>`
	`36`	`+#include <tesseract_msgs/CollisionMarginPairOverrideType.h>`
`37`	`37`	`#include <tesseract_msgs/ContactMarginPair.h>`
`38`	`38`	`#include <tesseract_msgs/ContactResultVector.h>`
`39`	`39`	`#include <tesseract_msgs/ContactManagersPluginInfo.h>`
`@@ -1028,15 +1028,15 @@ toMsg(const tesseract_common::PairsCollisionMarginData& contact_margin_pairs)`
`1028`	`1028`
`1029`	`1029`	`tesseract_common::CollisionMarginData fromMsg(const tesseract_msgs::CollisionMarginData& contact_margin_data_msg)`
`1030`	`1030`	`{`
`1031`		`- tesseract_common::PairsCollisionMarginData contact_margin_pairs = fromMsg(contact_margin_data_msg.margin_pairs);`
`1032`		`- return tesseract_common::CollisionMarginData(contact_margin_data_msg.default_margin, contact_margin_pairs);`
	`1031`	`+ tesseract_common::CollisionMarginPairData pair_data(fromMsg(contact_margin_data_msg.margin_pairs));`
	`1032`	`+ return tesseract_common::CollisionMarginData(contact_margin_data_msg.default_margin, pair_data);`
`1033`	`1033`	`}`
`1034`	`1034`
`1035`	`1035`	`tesseract_msgs::CollisionMarginData toMsg(const tesseract_common::CollisionMarginData& contact_margin_data)`
`1036`	`1036`	`{`
`1037`	`1037`	`tesseract_msgs::CollisionMarginData contact_margin_data_msg;`
`1038`	`1038`	`contact_margin_data_msg.default_margin = contact_margin_data.getDefaultCollisionMargin();`
`1039`		`- for (const auto& pair : contact_margin_data.getPairCollisionMargins())`
	`1039`	`+ for (const auto& pair : contact_margin_data.getCollisionMarginPairData().getCollisionMargins())`
`1040`	`1040`	`{`
`1041`	`1041`	`tesseract_msgs::ContactMarginPair cmp;`
`1042`	`1042`	`cmp.first.first = pair.first.first;`
`@@ -1047,80 +1047,53 @@ tesseract_msgs::CollisionMarginData toMsg(const tesseract_common::CollisionMargi`
`1047`	`1047`	`return contact_margin_data_msg;`
`1048`	`1048`	`}`
`1049`	`1049`
`1050`		`-tesseract_common::CollisionMarginOverrideType`
`1051`		`-fromMsg(const tesseract_msgs::CollisionMarginOverrideType& contact_margin_override_type_msg)`
	`1050`	`+tesseract_common::CollisionMarginPairOverrideType`
	`1051`	`+fromMsg(const tesseract_msgs::CollisionMarginPairOverrideType& contact_margin_pair_override_type_msg)`
`1052`	`1052`	`{`
`1053`		`- switch (contact_margin_override_type_msg.type)`
	`1053`	`+ switch (contact_margin_pair_override_type_msg.type)`
`1054`	`1054`	`{`
`1055`		`- case tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN:`
	`1055`	`+ case tesseract_msgs::CollisionMarginPairOverrideType::MODIFY:`
`1056`	`1056`	`{`
`1057`		`- return tesseract_common::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN;`
	`1057`	`+ return tesseract_common::CollisionMarginPairOverrideType::MODIFY;`
`1058`	`1058`	`}`
`1059`		`- case tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN:`
	`1059`	`+ case tesseract_msgs::CollisionMarginPairOverrideType::REPLACE:`
`1060`	`1060`	`{`
`1061`		`- return tesseract_common::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN;`
	`1061`	`+ return tesseract_common::CollisionMarginPairOverrideType::REPLACE;`
`1062`	`1062`	`}`
`1063`		`- case tesseract_msgs::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN:`
	`1063`	`+ case tesseract_msgs::CollisionMarginPairOverrideType::NONE:`
`1064`	`1064`	`{`
`1065`		`- return tesseract_common::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN;`
`1066`		`- }`
`1067`		`- case tesseract_msgs::CollisionMarginOverrideType::MODIFY:`
`1068`		`- {`
`1069`		`- return tesseract_common::CollisionMarginOverrideType::MODIFY;`
`1070`		`- }`
`1071`		`- case tesseract_msgs::CollisionMarginOverrideType::REPLACE:`
`1072`		`- {`
`1073`		`- return tesseract_common::CollisionMarginOverrideType::REPLACE;`
`1074`		`- }`
`1075`		`- case tesseract_msgs::CollisionMarginOverrideType::NONE:`
`1076`		`- {`
`1077`		`- return tesseract_common::CollisionMarginOverrideType::NONE;`
	`1065`	`+ return tesseract_common::CollisionMarginPairOverrideType::NONE;`
`1078`	`1066`	`}`
`1079`	`1067`	`default:`
`1080`	`1068`	`{`
`1081`		`- throw std::runtime_error("fromMsg: Invalid CollisionMarginOverrideType!");`
	`1069`	`+ throw std::runtime_error("fromMsg: Invalid CollisionMarginPairOverrideType!");`
`1082`	`1070`	`}`
`1083`	`1071`	`}`
`1084`	`1072`	`}`
`1085`	`1073`
`1086`		`-tesseract_msgs::CollisionMarginOverrideType`
`1087`		`-toMsg(const tesseract_common::CollisionMarginOverrideType& contact_margin_override_type)`
	`1074`	`+tesseract_msgs::CollisionMarginPairOverrideType`
	`1075`	`+toMsg(const tesseract_common::CollisionMarginPairOverrideType& contact_margin_pair_override_type)`
`1088`	`1076`	`{`
`1089`		`- tesseract_msgs::CollisionMarginOverrideType contact_margin_override_type_msg;`
`1090`		`- switch (static_cast<int>(contact_margin_override_type))`
	`1077`	`+ tesseract_msgs::CollisionMarginPairOverrideType contact_margin_pair_override_type_msg;`
	`1078`	`+ switch (static_cast<int>(contact_margin_pair_override_type))`
`1091`	`1079`	`{`
`1092`		`- case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN):`
	`1080`	`+ case static_cast<int>(tesseract_collision::CollisionMarginPairOverrideType::MODIFY):`
`1093`	`1081`	`{`
`1094`		`- contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN;`
	`1082`	`+ contact_margin_pair_override_type_msg.type = tesseract_msgs::CollisionMarginPairOverrideType::MODIFY;`
`1095`	`1083`	`break;`
`1096`	`1084`	`}`
`1097`		`- case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN):`
	`1085`	`+ case static_cast<int>(tesseract_collision::CollisionMarginPairOverrideType::REPLACE):`
`1098`	`1086`	`{`
`1099`		`- contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::OVERRIDE_PAIR_MARGIN;`
	`1087`	`+ contact_margin_pair_override_type_msg.type = tesseract_msgs::CollisionMarginPairOverrideType::REPLACE;`
`1100`	`1088`	`break;`
`1101`	`1089`	`}`
`1102`		`- case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN):`
	`1090`	`+ case static_cast<int>(tesseract_collision::CollisionMarginPairOverrideType::NONE):`
`1103`	`1091`	`{`
`1104`		`- contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::MODIFY_PAIR_MARGIN;`
`1105`		`- break;`
`1106`		`- }`
`1107`		`- case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::MODIFY):`
`1108`		`- {`
`1109`		`- contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::MODIFY;`
`1110`		`- break;`
`1111`		`- }`
`1112`		`- case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::REPLACE):`
`1113`		`- {`
`1114`		`- contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::REPLACE;`
`1115`		`- break;`
`1116`		`- }`
`1117`		`- case static_cast<int>(tesseract_collision::CollisionMarginOverrideType::NONE):`
`1118`		`- {`
`1119`		`- contact_margin_override_type_msg.type = tesseract_msgs::CollisionMarginOverrideType::NONE;`
	`1092`	`+ contact_margin_pair_override_type_msg.type = tesseract_msgs::CollisionMarginPairOverrideType::NONE;`
`1120`	`1093`	`break;`
`1121`	`1094`	`}`
`1122`	`1095`	`}`
`1123`		`- return contact_margin_override_type_msg;`
	`1096`	`+ return contact_margin_pair_override_type_msg;`
`1124`	`1097`	`}`
`1125`	`1098`
`1126`	`1099`	`bool toMsg(std::vector<tesseract_msgs::AllowedCollisionEntry>& acm_msg,`
`@@ -1350,8 +1323,14 @@ bool toMsg(tesseract_msgs::EnvironmentCommand& command_msg, const tesseract_envi`
`1350`	`1323`	`{`
`1351`	`1324`	`command_msg.command = tesseract_msgs::EnvironmentCommand::CHANGE_COLLISION_MARGINS;`
`1352`	`1325`	`const auto& cmd = static_cast<const tesseract_environment::ChangeCollisionMarginsCommand&>(command);`
`1353`		`- command_msg.collision_margin_data = toMsg(cmd.getCollisionMarginData());`
`1354`		`- command_msg.collision_margin_override_type = toMsg(cmd.getCollisionMarginOverrideType());`
	`1326`	`+`
	`1327`	`+ std::optional<double> default_margin = cmd.getDefaultCollisionMargin();`
	`1328`	`+ command_msg.has_collision_default_margin = default_margin.has_value();`
	`1329`	`+ if (default_margin.has_value())`
	`1330`	`+ command_msg.collision_default_margin = default_margin.value();`
	`1331`	`+`
	`1332`	`+ command_msg.collision_margin_pair_data = toMsg(cmd.getCollisionMarginPairData().getCollisionMargins());`
	`1333`	`+ command_msg.collision_margin_pair_override_type = toMsg(cmd.getCollisionMarginPairOverrideType());`
`1355`	`1334`	`return true;`
`1356`	`1335`	`}`
`1357`	`1336`	`case tesseract_environment::CommandType::ADD_CONTACT_MANAGERS_PLUGIN_INFO:`
`@@ -1538,10 +1517,18 @@ std::shared_ptr<tesseract_environment::Command> fromMsg(const tesseract_msgs::En`
`1538`	`1517`	`}`
`1539`	`1518`	`case tesseract_msgs::EnvironmentCommand::CHANGE_COLLISION_MARGINS:`
`1540`	`1519`	`{`
`1541`		`- tesseract_common::CollisionMarginData collision_margin_data = fromMsg(command_msg.collision_margin_data);`
`1542`		`- tesseract_common::CollisionMarginOverrideType override_type = fromMsg(command_msg.collision_margin_override_type);`
`1543`		`- return std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(collision_margin_data,`
`1544`		`- override_type);`
	`1520`	`+ tesseract_common::CollisionMarginPairData pair_margin_data(fromMsg(command_msg.collision_margin_pair_data));`
	`1521`	`+ tesseract_common::CollisionMarginPairOverrideType pair_override_type =`
	`1522`	`+ fromMsg(command_msg.collision_margin_pair_override_type);`
	`1523`	`+`
	`1524`	`+ if (command_msg.has_collision_default_margin)`
	`1525`	`+ {`
	`1526`	`+ return std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(`
	`1527`	`+ command_msg.collision_default_margin, pair_margin_data, pair_override_type);`
	`1528`	`+ }`
	`1529`	`+`
	`1530`	`+ return std::make_shared<tesseract_environment::ChangeCollisionMarginsCommand>(pair_margin_data,`
	`1531`	`+ pair_override_type);`
`1545`	`1532`	`}`
`1546`	`1533`	`case tesseract_msgs::EnvironmentCommand::ADD_CONTACT_MANAGERS_PLUGIN_INFO:`
`1547`	`1534`	`{`