Update to support Geometry Type CompoundMesh

tesseract-robotics · Sep 25, 2024 · dec68ed · dec68ed
1 parent fbd754d
commit dec68ed
Show file tree

Hide file tree

Showing 4 changed files with 408 additions and 452 deletions.
diff --git a/tesseract_msgs/msg/Geometry.msg b/tesseract_msgs/msg/Geometry.msg
@@ -2,15 +2,16 @@
 
 uint8 SPHERE=1
 uint8 CYLINDER=2
-uint8 CAPSULE=10
 uint8 CONE=3
 uint8 BOX=4
 uint8 PLANE=5
 uint8 MESH=6
-uint8 POLYGON_MESH=11
 uint8 CONVEX_MESH=7
 uint8 SDF_MESH=8
 uint8 OCTREE=9
+uint8 CAPSULE=10
+uint8 POLYGON_MESH=11
+uint8 COMPOUND_MESH=12
 
 # The type of the geometry
 uint8 type
@@ -33,9 +34,13 @@ float64[3] box_dimensions
 # PLANE
 float64[4] plane_coeff
 
-# MESH, POLYGON_MESH, CONVEX_MESH, SDF_MESH
+# MESH, CONVEX_MESH, SDF_MESH, POLYGON_MESH
 tesseract_msgs/Mesh mesh
 
 # OCTREE
 octomap_msgs/Octomap octomap
 tesseract_msgs/OctreeSubType octomap_sub_type
+
+# COMPOUND_MESH (MESH, CONVEX_MESH, SDF_MESH, POLYGON_MESH)
+tesseract_msgs/Mesh[] compound_mesh
+uint8 compound_mesh_type #(MESH, CONVEX_MESH, SDF_MESH, POLYGON_MESH)
diff --git a/tesseract_rosutils/include/tesseract_rosutils/utils.h b/tesseract_rosutils/include/tesseract_rosutils/utils.h
@@ -274,13 +274,19 @@ bool fromMsg(Eigen::Isometry3d& pose, const geometry_msgs::msg::Pose& pose_msg);
  */
 bool toMsg(geometry_msgs::msg::Pose& pose_msg, const Eigen::Isometry3d& pose);
 
+void toMsg(tesseract_msgs::msg::ContactResult& contact_result_msg,
+           const tesseract_collision::ContactResult& contact_result);
+
 void toMsg(tesseract_msgs::msg::ContactResult& contact_result_msg,
            const tesseract_collision::ContactResult& contact_result,
-           const rclcpp::Time& stamp = rclcpp::Clock{ RCL_ROS_TIME }.now());
+           const rclcpp::Time& stamp);
+
+void toMsg(const tesseract_msgs::msg::ContactResult::SharedPtr& contact_result_msg,
+           const tesseract_collision::ContactResult& contact_result);
 
 void toMsg(const tesseract_msgs::msg::ContactResult::SharedPtr& contact_result_msg,
            const tesseract_collision::ContactResult& contact_result,
-           const rclcpp::Time& stamp = rclcpp::Clock{ RCL_ROS_TIME }.now());
+           const rclcpp::Time& stamp);
 
 /**
  * @brief Convert kinematics plugin info to message