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

Author: sohils

cuboid_detection/launch/iterative_closest_point.launch

@@ -2,10 +2,13 @@
 <launch>
 
     <!-- Launch Bounding Box Filter -->
-    <include file="$(find cuboid_detection)/launch/bbox_filter.launch"/>
+    <!-- <include file="$(find cuboid_detection)/launch/bbox_filter.launch"/> -->
 
     <!-- Launch surface normal estimation -->
-    <include file="$(find cuboid_detection)/launch/surface_normal_estimation.launch"/>
+    <!-- <include file="$(find cuboid_detection)/launch/surface_normal_estimation.launch"/> -->
+
+    <!-- Launch ground plane segmentation -->
+    <include file="$(find cuboid_detection)/launch/ground_plane_segmentation.launch"/>
 
     <!-- Run ICP node -->
     <node
@@ -18,14 +21,14 @@
         <param
             name="template_cuboid_path"
             type="string"
-            value="$(find cuboid_detection)/templates/template_cuboid_L200_W75_H100_3faces.pcd" />
+            value="$(find cuboid_detection)/templates/template_cuboid_L200_W100_H30_3faces.pcd" />
 
         <!-- Set topics and params -->
         <rosparam>
             <!-- Dimensions -->
             length: 0.2
-            width: 0.075
-            height: 0.1
+            width: 0.1
+            height: 0.03
         </rosparam>
     </node>
 

cuboid_detection/src/iterative_closest_point.cpp

@@ -26,6 +26,8 @@ using namespace std;
 ros::Publisher pcl_pub;
 ros::Publisher bbox_pub;
 ros::Publisher template_pub;
+ros::Publisher pose_pub;
+
 Eigen::Matrix4d icp_transform;
 string template_cuboid_filename;
 sensor_msgs::PointCloud2 bbox_msg;
@@ -49,6 +51,41 @@ tf::Transform get_tf_from_stamped_tf(tf::StampedTransform sTf)
     return tf;
 }
 
+void publish_pose(Eigen::Matrix4d H)
+{
+    // Set translation
+    tf::Vector3 origin;
+    origin.setValue(H(0, 3), H(1, 3), H(2, 3));
+    
+    // Set rotation
+    tf::Quaternion quaternion;
+    tf::Matrix3x3 rotation_matrix;
+    rotation_matrix.setValue(H(0, 0), H(0, 1), H(0, 2),  
+                             H(1, 0), H(1, 1), H(1, 2),  
+                             H(2, 0), H(2, 1), H(2, 2));
+    rotation_matrix.getRotation(quaternion);
+
+    // Make tf transform message
+    tf::Transform transform;
+    transform.setOrigin(origin);
+    transform.setRotation(quaternion);
+
+    // Make pose message
+    geometry_msgs::Pose p;
+    p.position.x = H(0, 3);
+    p.position.y = H(1, 3);
+    p.position.z = H(2, 3);
+    p.orientation.x = quaternion.x();
+    p.orientation.y = quaternion.y();
+    p.orientation.z = quaternion.z();
+    p.orientation.w = quaternion.w();
+
+    // Broadcast the transforms
+    static tf::TransformBroadcaster br;
+    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_depth_optical_frame", "icp_cuboid_frame"));
+    pose_pub.publish(p);
+}
+
 void publish_bounding_box(Eigen::Matrix4d H)
 {
     // Extract cuboid dimensions
@@ -98,14 +135,15 @@ void pose_callback(const geometry_msgs::Pose::ConstPtr& msg)
 void icp_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
 {
     // Compute ICP only once
-    // if (ICP_SUCCESS)
-    // {
-    //     pcl_pub.publish(output_msg);
-    //     template_msg.header.frame_id = "camera_depth_optical_frame";
-    //     template_pub.publish(template_msg);
-    //     publish_bounding_box(icp_transform);
-    //     return;
-    // }
+    if (ICP_SUCCESS)
+    {
+        pcl_pub.publish(output_msg);
+        template_msg.header.frame_id = "camera_depth_optical_frame";
+        template_pub.publish(template_msg);
+        publish_bounding_box(icp_transform);
+        publish_pose(icp_transform);
+        return;
+    }
 
     // Point cloud containers
     pcl::PointCloud<pcl::PointXYZ>::Ptr template_input(new pcl::PointCloud<pcl::PointXYZ>);
@@ -117,15 +155,15 @@ void icp_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
     pcl::fromROSMsg(*msg, *input_cuboid);
 
     // Read template point cloud
-    if (pcl::io::loadPCDFile<pcl::PointXYZ>(template_cuboid_filename, *template_input) == -1)
+    if (pcl::io::loadPCDFile<pcl::PointXYZ>(template_cuboid_filename, *template_cuboid) == -1)
     {
         PCL_ERROR("Couldn't read the template PCL file");
         return;
     }
 
     // Transform the template point cloud to the estimated frame
-    if (!POSE_FLAG) return;
-    pcl::transformPointCloud(*template_input, *template_cuboid, pose_transform.cast<float>());
+    // if (!POSE_FLAG) return;
+    // pcl::transformPointCloud(*template_input, *template_cuboid, pose_transform.cast<float>());
 
     // Run ICP
     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
@@ -143,22 +181,23 @@ void icp_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
     if (icp.hasConverged())
     {
         // Display ICP results
-        ICP_SUCCESS = true;
         if (DEBUG || !ICP_SUCCESS)
         {
             cerr << "\nICP Score: " << icp.getFitnessScore() << endl;
             cerr << "ICP Transform:\n" << icp_transform << endl;
         }
+        ICP_SUCCESS = true;
+
+        // Convert to ROS data type
+        pcl::toROSMsg(*output_cloud, output_msg);
+        pcl::toROSMsg(*template_cuboid, template_msg);
 
         // Publish template point cloud
         template_msg.header.frame_id = "camera_depth_optical_frame";
         pcl_pub.publish(output_msg);
         template_pub.publish(template_msg);
         publish_bounding_box(icp_transform);
-
-        // Convert to ROS data type
-        pcl::toROSMsg(*output_cloud, output_msg);
-        pcl::toROSMsg(*template_cuboid, template_msg);
+        publish_pose(icp_transform);
     }
 }
 
@@ -188,6 +227,7 @@ int main(int argc, char **argv)
     pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/aligned_points", 1);
     bbox_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/bbox_points", 1);
     template_pub = nh.advertise<sensor_msgs::PointCloud2>("/icp/template", 1);
+    pose_pub = nh.advertise<geometry_msgs::Pose>("/icp/pose", 1);
 
     // Spin
     ros::spin();