Merge branch 'master' of github.com:dash-robotics/perception

Author: sohils

cuboid_detection/launch/iterative_closest_point.launch

@@ -61,11 +61,11 @@
     </node>
 
     <!-- Run RVIZ -->
-    <node
+    <!--node
         type="rviz"
         name="rviz"
         pkg="rviz"
         args="-d $(find cuboid_detection)/perception.rviz">
-    </node>
+    </node-->
 
 </launch>

object_detection/CMakeLists.txt

@@ -65,11 +65,10 @@ include_directories(${OpenCV_INCLUDE_DIRS})
 # )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  ObjectDetection.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -145,8 +144,7 @@ include
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(ground_plane_segmentation src/ground_plane_segmentation.cpp)
-add_executable(iterative_closest_point src/iterative_closest_point.cpp)
+add_executable(object_pose_detection src/object_pose_detection.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -159,5 +157,4 @@ add_executable(iterative_closest_point src/iterative_closest_point.cpp)
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(ground_plane_segmentation ${catkin_LIBRARIES})
-target_link_libraries(iterative_closest_point ${catkin_LIBRARIES})
+target_link_libraries(object_pose_detection ${catkin_LIBRARIES})

object_detection/launch/ground_plane_segmentation.launch

@@ -2,26 +2,42 @@
 <launch>
 
     <!-- Play rosbag file -->
-    <include file="$(find object_detection)/launch/play_rosbag.launch">
+    <!-- <include file="$(find object_detection)/launch/play_rosbag.launch">
         <arg name="filename" value="all-01.bag" />
-    </include>
+    </include> -->
 
     <!-- Launch the depth camera -->
     <!-- <include file="$(find realsense2_camera)/launch/rs_camera.launch">
         <arg name="enable_pointcloud" value="true"/>
     </include> -->
 
-    <!-- Launch Bounding Box Filter -->
-    <!-- <include file="$(find object_detection)/launch/bbox_filter.launch"/> -->
-    
-    <!-- Launch surface normal estimation -->
-    <!-- <include file="$(find object_detection)/launch/surface_normal_estimation.launch"/> -->
+    <!-- Launch object pose detection -->
+    <node
+        pkg="object_detection"
+        type="object_pose_detection"
+        name="object_pose_detection"
+        output="screen">
 
-    <!-- Launch ground plane segmentation -->
-    <include file="$(find object_detection)/launch/ground_plane_segmentation.launch"/>
+        <!-- Template object folder for ICP -->
+        <param
+            name="template_path"
+            type="string"
+            value="$(find object_detection)/templates/" />
 
-    <!-- Launch ICP -->
-    <!-- <include file="$(find object_detection)/launch/iterative_closest_point.launch"/> -->
+        <!-- Set topics and params -->
+        <rosparam>
+            <!-- invert (default: true): True > Box; False > Ground Plane -->
+            invert: true
+            <!-- voxel_size (default: 0.01): Lower the value, denser the point cloud, more CPU -->
+            voxel_size: 0.001
+            <!-- distance_threshold (default: 0.01): Lower the value, more of box + table noise -->
+            distance_threshold: 0.01
+            <!-- Topics -->
+            input: "/camera/depth/color/points"
+            output: "/object_pose_detection/points"
+            icp_fitness_score: 0.0004
+        </rosparam>
+    </node>
 
     <!-- Display the 3D bounding box -->
     <!-- <node
@@ -40,11 +56,11 @@
     </node> -->
 
     <!-- Run RVIZ -->
-    <node
+    <!-- <node
         type="rviz"
         name="rviz"
         pkg="rviz"
         args="-d $(find object_detection)/perception.rviz">
-    </node>
+    </node> -->
 
 </launch>

object_detection/launch/object_detection.launch

@@ -3,7 +3,8 @@ Panels:
     Help Height: 75
     Name: Displays
     Property Tree Widget:
-      Expanded: ~
+      Expanded:
+        - /Axes1
       Splitter Ratio: 0.5
     Tree Height: 371
   - Class: rviz/Selection
@@ -24,7 +25,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Raw Input PCL
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -51,7 +52,7 @@ Visualization Manager:
       Length: 0.5
       Name: Axes
       Radius: 0.00999999978
-      Reference Frame: <Fixed Frame>
+      Reference Frame: camera_depth_optical_frame
       Value: true
     - Class: rviz/Image
       Enabled: true
@@ -77,7 +78,7 @@ Visualization Manager:
       Color: 255; 0; 0
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -94,7 +95,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -107,7 +108,7 @@ Visualization Manager:
       Color: 0; 255; 0
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -120,11 +121,11 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.00999999978
       Style: Points
-      Topic: /icp/aligned_points
+      Topic: /icp/registered_pcl
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -210,11 +211,19 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.00999999978
       Style: Points
-      Topic: /ground_plane_segmentation/points
+      Topic: /object_pose_detection/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /object_pose_detection/object_pose_detection/grasp_pose
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -240,25 +249,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.469594628
+      Distance: 0.911043346
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.33194834
-        Y: 0.230934247
-        Z: -0.0767720714
+        X: -0.012347864
+        Y: -0.153358907
+        Z: -0.142543241
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.320203245
+      Pitch: 0.550203443
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.77925992
+      Yaw: 2.59425759
     Saved: ~
 Window Geometry:
   3D Bounding Box Image: