bbox work

CMakeLists.txt

@@ -19,11 +19,11 @@ find_package(catkin REQUIRED COMPONENTS
   actionlib_msgs
   moveit_msgs
   cmake_modules
-  pcl_ros
 )
 
 find_package(Eigen REQUIRED)
 find_package(Boost REQUIRED thread system)
+find_package(PCL 1.8 REQUIRED)
 
 add_message_files(DIRECTORY msg FILES
   GraspGeneratorOptions.msg
@@ -60,8 +60,12 @@ catkin_package(
 include_directories(
   include 
   ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
 )
 
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
 # Grasp Generator Library
 add_library(${PROJECT_NAME}
   src/grasps.cpp
@@ -100,7 +104,7 @@ target_link_libraries(cuboid_grasp_test
 # Test mesh bounding box
 add_executable(bounding_box_test src/mesh_bounding_box_test.cpp)
 target_link_libraries(bounding_box_test
-  ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}
+  ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PCL_LIBRARIES}
 )
 
 ## Install ---------------------------------------------------------------

README.md

@@ -232,3 +232,34 @@ Features we'd like to see added to this project:
 
  - Dave Coleman, CU Boulder @davetcoleman
  - Bence Magyar, PAL Robotics @bmagyar
+
+#PCL Junk
+
+install pcl from source:
+```
+git clone [email protected]:PointCloudLibrary/pcl.git
+cd pcl
+mkdir build
+cd build
+cmake -DCMAKE_BUILD_TYPE=Release .. 
+make
+sudo make install
+```
+Read the output and see where it gets installed. Mine was `/usr/local/include/pcl-1.8`
+
+check if you have local include in 'LD_LIBRARY_PATH'
+```
+echo $LD_LIBRARY_PATH
+```
+
+if not, add to your `.bashrc`
+```
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/include
+```
+
+for files that need it:
+```
+#include <pcl-1.8/pcl/features/moment_of_inertia_estimation.h>
+```
+
+Edit `CMakeLists` per [this ROS Answer](#include <pcl-1.8/pcl/features/moment_of_inertia_estimation.h>)

config/jacob_grasp_data.yaml

@@ -16,13 +16,13 @@ gripper:
     grasp_time_from_start : 4.0
 
     #distance from wrist joint to palm of end effector
-    grasp_pose_to_eef_translation :  [0, 0, 0.1]
+    grasp_pose_to_eef_translation :  [0, 0, -0.1]
 
     #Rotation from wrist joint to std end effector orientation
     #z-axis pointing toward object to grasp
     #x-axis perp. to movement of grippers
     #y-axis parallel to movement of grippers
-    grasp_pose_to_eef_rotation : [-1.5708, 0, 0]
+    grasp_pose_to_eef_rotation : [-1.5708, 3.14159, 0]
 
     # length of grippers (distance from finger tip to inner palm)
     finger_to_palm_depth : 0.11

package.xml

@@ -33,7 +33,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>cmake_modules</build_depend>
-  <build_depend>pcl_ros</build_depend>
+  <build_depend>pcl17</build_depend>
 
   <run_depend>std_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>
@@ -42,6 +42,6 @@
   <run_depend>actionlib_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>moveit_visual_tools</run_depend>
-  <run_depend>pcl_ros</run_depend>
+  <run_depend>pcl17</run_depend>
 
 </package>

src/grasps.cpp

@@ -601,7 +601,6 @@ bool Grasps::generateCuboidAxisGrasps(const Eigen::Affine3d& cuboid_pose, float
       rotation_angle *= -1;
 
     grasp_pose = grasp_pose * Eigen::AngleAxisd(rotation_angle, Eigen::Vector3d::UnitY());
-    grasp_pose = grasp_pose * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
 
     if (verbose_)
     {

src/mesh_bounding_box_test.cpp

@@ -12,7 +12,8 @@
 #include <geometric_shapes/shape_operations.h>
 #include <geometric_shapes/bodies.h>
 
-//#include <pcl17/features/moment_of_inertia_estimation.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/common/common_headers.h>
 
 #include <boost/filesystem.hpp>
 
@@ -21,6 +22,54 @@ namespace fs = boost::filesystem;
 namespace moveit_grasps
 {
 
+bool publishWireframeCuboid(const Eigen::Vector3f &position,
+                            const Eigen::Quaternionf &quat,
+                            const Eigen::Vector3f &min_point,
+                            const Eigen::Vector3f &max_point) {
+  Eigen::Vector3f p1 (min_point.x, min_point.y, min_point.z);
+  Eigen::Vector3f p2 (min_point.x, min_point.y, max_point.z);
+  Eigen::Vector3f p3 (max_point.x, min_point.y, max_point.z);
+  Eigen::Vector3f p4 (max_point.x, min_point.y, min_point.z);
+  Eigen::Vector3f p5 (min_point.x, max_point.y, min_point.z);
+  Eigen::Vector3f p6 (min_point.x, max_point.y, max_point.z);
+  Eigen::Vector3f p7 (max_point.x, max_point.y, max_point.z);
+  Eigen::Vector3f p8 (max_point.x, max_point.y, min_point.z);
+
+  p1 = rotational_matrix_OBB * p1 + position;
+  p2 = rotational_matrix_OBB * p2 + position;
+  p3 = rotational_matrix_OBB * p3 + position;
+  p4 = rotational_matrix_OBB * p4 + position;
+  p5 = rotational_matrix_OBB * p5 + position;
+  p6 = rotational_matrix_OBB * p6 + position;
+  p7 = rotational_matrix_OBB * p7 + position;
+  p8 = rotational_matrix_OBB * p8 + position;
+
+  pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2));
+  pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2));
+  pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2));
+  pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2));
+  pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2));
+  pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2));
+  pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2));
+  pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2));
+
+  publishLine(pt1, pt2, 1.0, 0.0, 0.0, "1 edge");
+  publishLine(pt1, pt4, 1.0, 0.0, 0.0, "2 edge");
+  publishLine(pt1, pt5, 1.0, 0.0, 0.0, "3 edge");
+  publishLine(pt5, pt6, 1.0, 0.0, 0.0, "4 edge");
+  publishLine(pt5, pt8, 1.0, 0.0, 0.0, "5 edge");
+  publishLine(pt2, pt6, 1.0, 0.0, 0.0, "6 edge");
+  publishLine(pt6, pt7, 1.0, 0.0, 0.0, "7 edge");
+  publishLine(pt7, pt8, 1.0, 0.0, 0.0, "8 edge");
+  publishLine(pt2, pt3, 1.0, 0.0, 0.0, "9 edge");
+  publishLine(pt4, pt8, 1.0, 0.0, 0.0, "10 edge");
+  publishLine(pt3, pt4, 1.0, 0.0, 0.0, "11 edge");
+  publishLine(pt3, pt7, 1.0, 0.0, 0.0, "12 edge");
+
+  return true;
+}
+
+
 class MeshBoundingBoxTest
 {
 private:
@@ -63,16 +112,16 @@ class MeshBoundingBoxTest
       visual_tools_->publishMesh(mesh_pose_,mesh_path.string());
 
       // GET BOUNDING BOX FUNCTION
-<<<<<<< Updated upstream
-      shapes::Shape *mesh = shapes::createMeshFromResource(mesh_path.string());
-=======
-      shapes::Shape* mesh = shapes::createMeshFromResource(mesh_path.string());
-
-      const bodies::Body* body = new bodies::ConvexMesh(mesh);
+      shapes::Shape* mesh= shapes::createMeshFromResource(mesh_path.string());
+      shapes::ShapeMsg shape_msg;
+      if (!mesh || !shapes::constructMsgFromShape(mesh, shape_msg))
+      {
+        ROS_ERROR_STREAM_NAMED("bounding box test","Unable to create mesh shape message from resource " << mesh_path);
+      }
+      shape_msgs::Mesh mesh_msg = boost::get<shape_msgs::Mesh>(shape_msg);
 
       double depth, width, height;
-      computeBoundingBox(body, depth, width, height);
->>>>>>> Stashed changes
+      computeBoundingBox(mesh_msg, depth, width, height);
 
       // END BOUNDING BOX FUNCTION
 
@@ -83,25 +132,54 @@ class MeshBoundingBoxTest
 
   }
 
-<<<<<<< Updated upstream
-=======
-  void computeBoundingBox(const bodies::Body* body, double& depth, double& width, double& height)
+  void computeBoundingBox(shape_msgs::Mesh mesh_msg, double& depth, double& width, double& height)
   {
-    const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(body);
-
-    size_t number_of_vertices = conv->getScaledVertices().size();
-    ROS_DEBUG_STREAM_NAMED("bounding_box","Number of vertices = " << number_of_vertices);
+
+    int num_vertices = mesh_msg.vertices.size();
+    ROS_DEBUG_STREAM_NAMED("bbox","num triangles = " << mesh_msg.triangles.size());
+    ROS_DEBUG_STREAM_NAMED("bbox","num vertices = " << num_vertices);
 
-    double xmin, xmax, ymin, ymax, zmin, zmax;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointXYZ point;
 
-    for (int i = 0; i < number_of_vertices; i++)
+    // get vertices into point cloud for pcl magic
+    for (int i = 0; i < num_vertices; i++)
     {
-
+      point.x = mesh_msg.vertices[i].x;
+      point.y = mesh_msg.vertices[i].y;
+      point.z = mesh_msg.vertices[i].z;
+      cloud->points.push_back(point);
     }
 
+    ROS_DEBUG_STREAM_NAMED("bbox","num points in cloud = " << cloud->points.size());
+
+    // extract bounding box properties
+    ROS_DEBUG_STREAM_NAMED("bbox","starting cloud feature extractor");
+
+    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
+    feature_extractor.setInputCloud(cloud);
+    feature_extractor.compute();
+
+    // Oriented BoundingBox
+    //    pcl::PointXYZ min_point_OBB;
+    //    pcl::PointXYZ max_point_OBB;
+    //    pcl::PointXYZ position_OBB;
+    //    Eigen::Matrix3f rotational_matrix_OBB;
+
+    //    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
+
+    //    Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
+    //    Eigen::Quaternionf quat (rotational_matrix_OBB);
+
+    pcl::PointXYZ min_point_AABB;
+    pcl::PointXYZ max_point_AABB;
+
+    feature_extractor.getAABB(min_point_AABB, max_point_AABB);
+
+    ROS_DEBUG_STREAM_NAMED("bbox","done extracting features");
+
   }
 
->>>>>>> Stashed changes
   double fRand(double fMin, double fMax) 
   {
     return fMin + ( (double)rand() / RAND_MAX ) * (fMax - fMin);