tesseract-robotics · Levi-Armstrong · Sep 25, 2019 · Aug 19, 2019 · Aug 20, 2019 · Aug 23, 2019
diff --git a/gh_pages/_source/tesseract_urdf_doc.rst b/gh_pages/_source/tesseract_urdf_doc.rst
@@ -0,0 +1,160 @@
+**************************
+Tesseract Geometry Package
+**************************
+
+Background
+==========
+This package contains urdf parser used by Tesseract. It supports additional shape and features not supported by urdfdom. This wiki only contains additional items and for more information please refer to http://wiki.ros.org/urdf/XML.
+
+Features
+========
+
+#. New Shapes
+
+   * Capsule
+   * Mesh
+   * Convex Mesh
+   * SDF Mesh
+   * Octomap
+
+#. Origin
+
+   * Quaternion
+
+Defining New Shapes
+===================
+
+Create Capsule
+--------------
+
+.. code-block:: xml
+
+   <capsule radius="1" length="2"/>
+
+The total height is the **length + 2 * radius**, so the length is just the height between the center of each sphere of the capsule caps.
+
+Create Convex Mesh
+------------------
+
+.. code-block:: xml
+
+   <convex_mesh filename="package://tesseract_support/meshes/box_2m.ply" scale="1 2 1" convert="false"/>
+
+This will create a convex hull shape type. This shape is more efficient than a regular mesh for collision checking. Also it provides an accurate penetration distance where in the case of mesh type you only get the penetration of one triangle into another.
+
+.. list-table::
+   :widths: 25 25 50
+   :header-rows: 1
+
+   * - Parameter
+     - Required/Optional
+     - Description
+   * - filename
+     - Required
+     - If convert is false (default) the mesh must be a convex hull represented by a polygon mesh. If it is triangulated such that multiple triangles represent the same surface you will get undefined behavior from collision checking.
+   * - scale
+     - Optional
+     - Scales the mesh axis aligned bounding box. Default scale = [1, 1, 1].
+   * - convert
+     - Optional
+     - If true the mesh is converted to a convex hull. Default convert = false.
+
+Create SDF Mesh
+---------------
+
+.. code-block:: xml
+
+   <sdf_mesh filename="package://tesseract_support/meshes/box_2m.ply" scale="1 2 1" />
+
+This will create a signed distance field shape type, which only affects collision shapes. This shape is more efficient than a regular mesh for collision checking, but not as efficient as convex hull.
+
+.. list-table::
+   :widths: 25 25 50
+   :header-rows: 1
+
+   * - Parameter
+     - Required/Optional
+     - Description
+   * - filename
+     - Required
+     - A path to a convex or non-convex mesh.
+   * - scale
+     - Optional
+     - Scales the mesh axis aligned bounding box. Default scale = [1, 1, 1].
+
+Create Octree/Octomap
+---------------------
+
+There are two methods for creating an octomap collision object. The first is to provide and octree file (.bt | .ot) and the second option is to provide a point cloud file (.pcd) with a resolution.
+
+.. code-block:: xml
+
+   <octomap shape_type="box" prune="false" >
+     <octree filename="package://tesseract_support/meshes/box_2m.bt"/>
+   </octomap>
+
+   <octomap shape_type="box" prune="false" >
+     <point_cloud filename="package://tesseract_support/meshes/box_2m.pcd" resolution="0.1"/>
+   </octomap>
+
+
+This will create an octomap shape type. Each occupied cell is represented by either a box, shere outside, or sphere inside shape.
+
+.. list-table:: Octomap Element
+   :widths: 25 25 50
+   :header-rows: 1
+
+   * - Parameter
+     - Required/Optional
+     - Description
+   * - shape_type
+     - Required
+     - Currently three shape types (box, sphere_inside, sphere_outside).
+   * - prune
+     - Optional
+     - This executes the octree toMaxLikelihood() the prune() method prior to creating shape which will combine adjacent occupied cell into larget cells resulting in fewer shapes.
+
+.. list-table:: Octree Element
+   :widths: 25 25 50
+   :header-rows: 1
+
+   * - Parameter
+     - Required/Optional
+     - Description
+   * - filename
+     - Required
+     - A path to a binary or ascii octree file.
+
+.. list-table:: Point Cloud Element
+   :widths: 25 25 50
+   :header-rows: 1
+
+   * - Parameter
+     - Required/Optional
+     - Description
+   * - filename
+     - Required
+     - A path to a PCL point clound file.
+   * - resolution
+     - Required
+     - The resolution of the octree populated by the provided point cloud
+
+Create Origin
+-------------
+
+.. code-block:: xml
+
+   <origin xyz="0 0 0" rpy="0 0 0" wxyz="1 0 0 0"/>;
+
+This allows the ability to use a quaternion instead of roll, pitch and yaw values. It is acceptable to have both to allow backwards compatability with other parsers, but the quaternion will take preference over rpy.
+
+.. list-table::
+   :widths: 25 25 50
+   :header-rows: 1
+
+   * - Parameter
+     - Required/Optional
+     - Description
+   * - wxyz
+     - Optional
+     - A Quaternion = [w, x, y, z]. It will be normalized on creation.
diff --git a/gh_pages/index.rst b/gh_pages/index.rst
@@ -22,6 +22,7 @@ Tesseract Core Packages
 * **tesseract_scene_graph** – This package contains the scene graph which is the data structure used to manage the connectivity of objects in the environment. It inherits from boost graph and provides addition functionality for adding,removing and modifying Links and Joints along with search implementation.
 * **tesseract_support** – This package contains support data used for unit tests and examples throughout Tesseract.
 * **tesseract_visualization** – This package contains visualization utilities and libraries.
+* **tesseract_urdf** - This package contains a custom urdf parser supporting addition shapes and features currently not supported by urdfdom.
 
 Tesseract ROS Packages
 ----------------------
@@ -50,6 +51,7 @@ Packages
    tesseract_rviz <_source/tesseract_rviz_doc.rst>
    tesseract_monitoring <_source/tesseract_monitoring_doc.rst>
    tesseract_planning <_source/tesseract_planning_doc.rst>
+   tesseract_urdf <_source/tesseract_urdf_doc.rst>
 
 FAQ
 ---

diff --git a/tesseract/tesseract/CMakeLists.txt b/tesseract/tesseract/CMakeLists.txt
@@ -8,12 +8,13 @@ find_package(tesseract_collision REQUIRED)
 find_package(tesseract_environment REQUIRED)
 find_package(tesseract_kinematics REQUIRED)
 find_package(tesseract_common REQUIRED)
+find_package(tesseract_urdf REQUIRED)
 
 list(FIND CMAKE_CXX_COMPILE_FEATURES cxx_std_11 CXX_FEATURE_FOUND)
 
 # Create target
 add_library(${PROJECT_NAME} SHARED src/tesseract.cpp)
-target_link_libraries(${PROJECT_NAME} PUBLIC tesseract::tesseract_scene_graph tesseract::tesseract_collision_bullet tesseract::tesseract_environment_kdl tesseract::tesseract_kinematics_kdl console_bridge)
+target_link_libraries(${PROJECT_NAME} PUBLIC tesseract::tesseract_urdf tesseract::tesseract_scene_graph tesseract::tesseract_collision_bullet tesseract::tesseract_environment_kdl tesseract::tesseract_kinematics_kdl console_bridge)
 target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wsuggest-override -Wconversion -Wsign-conversion)
 if(CXX_FEATURE_FOUND EQUAL "-1")
     target_compile_options(${PROJECT_NAME} PUBLIC -std=c++11)

diff --git a/tesseract/tesseract/cmake/tesseract-config.cmake.in b/tesseract/tesseract/cmake/tesseract-config.cmake.in
@@ -16,5 +16,7 @@ find_dependency(tesseract_collision)
 find_dependency(tesseract_environment)
 find_dependency(tesseract_kinematics)
 find_dependency(tesseract_common)
+find_dependency(tesseract_urdf)
+find_dependency(console_bridge)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
diff --git a/tesseract/tesseract/package.xml b/tesseract/tesseract/package.xml
@@ -12,6 +12,7 @@
   <depend>tesseract_kinematics</depend>
   <depend>tesseract_environment</depend>
   <depend>tesseract_common</depend>
+  <depend>tesseract_urdf</depend>
   <depend>libconsole-bridge-dev</depend>
 
   <export>

diff --git a/tesseract/tesseract/src/tesseract.cpp b/tesseract/tesseract/src/tesseract.cpp
@@ -33,7 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tesseract_kinematics/kdl/kdl_fwd_kin_tree_factory.h>
 #include <tesseract_kinematics/kdl/kdl_inv_kin_chain_lma_factory.h>
 #include <tesseract_kinematics/core/utils.h>
-#include <tesseract_scene_graph/parser/urdf_parser.h>
+#include <tesseract_urdf/urdf_parser.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract/tesseract.h>
@@ -94,7 +94,7 @@ bool Tesseract::init(const std::string& urdf_string, tesseract_scene_graph::Reso
   clear();
 
   // Parse urdf string into Scene Graph
-  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_scene_graph::parseURDFString(urdf_string, locator);
+  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_urdf::parseURDFString(urdf_string, locator);
   if (scene_graph == nullptr)
   {
     CONSOLE_BRIDGE_logError("Failed to parse URDF.");
@@ -124,7 +124,7 @@ bool Tesseract::init(const std::string& urdf_string,
   clear();
 
   // Parse urdf string into Scene Graph
-  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_scene_graph::parseURDFString(urdf_string, locator);
+  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_urdf::parseURDFString(urdf_string, locator);
   if (scene_graph == nullptr)
   {
     CONSOLE_BRIDGE_logError("Failed to parse URDF.");
@@ -164,8 +164,7 @@ bool Tesseract::init(const boost::filesystem::path& urdf_path, tesseract_scene_g
   clear();
 
   // Parse urdf file into Scene Graph
-  tesseract_scene_graph::SceneGraph::Ptr scene_graph =
-      tesseract_scene_graph::parseURDFFile(urdf_path.string(), locator);
+  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_urdf::parseURDFFile(urdf_path.string(), locator);
   if (scene_graph == nullptr)
   {
     CONSOLE_BRIDGE_logError("Failed to parse URDF.");
@@ -195,8 +194,7 @@ bool Tesseract::init(const boost::filesystem::path& urdf_path,
   clear();
 
   // Parse urdf file into Scene Graph
-  tesseract_scene_graph::SceneGraph::Ptr scene_graph =
-      tesseract_scene_graph::parseURDFFile(urdf_path.string(), locator);
+  tesseract_scene_graph::SceneGraph::Ptr scene_graph = tesseract_urdf::parseURDFFile(urdf_path.string(), locator);
   if (scene_graph == nullptr)
   {
     CONSOLE_BRIDGE_logError("Failed to parse URDF.");

diff --git a/tesseract/tesseract_collision/include/tesseract_collision/core/common.h b/tesseract/tesseract_collision/include/tesseract_collision/core/common.h
@@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_common/utils.h>
 #include <tesseract_collision/core/types.h>
 #include <tesseract_collision/core/common.h>
 
@@ -170,16 +171,18 @@ inline int createConvexHull(tesseract_common::VectorVector3d& vertices,
   vertices.clear();
 
   btConvexHullComputer conv;
-  btAlignedObjectArray<btVector3> points;
-  points.reserve(static_cast<int>(input.size()));
+  std::vector<double> points;
+  points.reserve(static_cast<int>(input.size() * 3));
   for (const auto& v : input)
   {
-    points.push_back(btVector3(static_cast<btScalar>(v[0]), static_cast<btScalar>(v[1]), static_cast<btScalar>(v[2])));
+    points.push_back(v[0]);
+    points.push_back(v[1]);
+    points.push_back(v[2]);
   }
 
-  btScalar val = conv.compute(&points[0].getX(),
-                              sizeof(btVector3),
-                              points.size(),
+  btScalar val = conv.compute(points.data(),
+                              3 * sizeof(double),
+                              input.size(),
                               static_cast<btScalar>(shrink),
                               static_cast<btScalar>(shrinkClamp));
   if (val < 0)
@@ -373,22 +376,6 @@ inline bool writeSimplePlyFile(const std::string& path,
   return writeSimplePlyFile(path, vertices, vertices_color, faces, num_faces);
 }
 
-/**
- * @brief Determine if a string is a number
- * @param s The string to evaluate
- * @return True if numeric, otherwise false.
- */
-inline bool isNumeric(const std::string& s)
-{
-  if (s.empty())
-    return false;
-
-  if (s[0] == '-')
-    return std::find_if(s.begin() + 1, s.end(), [](char c) { return !std::isdigit(c); }) == s.end();
-  else
-    return std::find_if(s.begin(), s.end(), [](char c) { return !std::isdigit(c); }) == s.end();
-}
-
 /**
  * @brief Loads a simple ply file given a path
  * @param path The file path
@@ -444,7 +431,7 @@ inline int loadSimplePlyFile(const std::string& path,
   std::getline(myfile, str);
   std::vector<std::string> tokens;
   boost::split(tokens, str, boost::is_any_of(" "));
-  if (tokens.size() != 3 || !isNumeric(tokens.back()))
+  if (tokens.size() != 3 || !tesseract_common::isNumeric(tokens.back()))
   {
     CONSOLE_BRIDGE_logError("Failed to parse file: %s", path.c_str());
     return false;
@@ -458,7 +445,7 @@ inline int loadSimplePlyFile(const std::string& path,
 
   tokens.clear();
   boost::split(tokens, str, boost::is_any_of(" "));
-  if (tokens.size() != 3 || !isNumeric(tokens.back()))
+  if (tokens.size() != 3 || !tesseract_common::isNumeric(tokens.back()))
   {
     CONSOLE_BRIDGE_logError("Failed to parse file: %s", path.c_str());
     return false;

diff --git a/tesseract/tesseract_collision/test/collision_octomap_mesh_unit.cpp b/tesseract/tesseract_collision/test/collision_octomap_mesh_unit.cpp
@@ -3,7 +3,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <octomap/octomap.h>
 #include <console_bridge/console.h>
 #include <gtest/gtest.h>
-#include <tesseract_scene_graph/parser/mesh_parser.h>
+#include <tesseract_geometry/mesh_parser.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include "tesseract_collision/bullet/bullet_discrete_simple_manager.h"
@@ -13,7 +13,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 using namespace tesseract_collision;
 using namespace tesseract_geometry;
-using namespace tesseract_scene_graph;
 
 void addCollisionObjects(DiscreteContactManager& checker)
 {

diff --git a/tesseract/tesseract_common/CMakeLists.txt b/tesseract/tesseract_common/CMakeLists.txt
@@ -49,3 +49,12 @@ install(FILES
   DESTINATION lib/cmake/${PROJECT_NAME})
 
 export(EXPORT ${PROJECT_NAME}-targets FILE ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-targets.cmake)
+
+if (ENABLE_TESTS)
+  enable_testing()
+  add_custom_target(run_tests ALL
+      WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+      COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> -V)
+
+  add_subdirectory(test)
+endif()