diff --git a/godel_process_planning/src/blend_process_planning.cpp b/godel_process_planning/src/blend_process_planning.cpp
index 374738aa..57bc549d 100644
--- a/godel_process_planning/src/blend_process_planning.cpp
+++ b/godel_process_planning/src/blend_process_planning.cpp
@@ -153,6 +153,9 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin
     return false;
   }
 
+  // Enable Collision Checks
+  blend_model_->setCheckCollisions(true);
+
   // Transform process path from geometry msgs to descartes points
   DescartesTraj process_points = toDescartesTraj(req.path.poses, req.params);
 
@@ -231,6 +234,17 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin
                                solved_path.end() - from_process.size());
     trajectory_msgs::JointTrajectory process = toROSTrajectory(process_part, *blend_model_);
 
+    for (std::size_t i = 0; i < process.points.size(); ++i)
+    {
+      const auto& pt = process.points[i];
+      if (!blend_model_->isValid(pt.positions))
+      {
+        ROS_WARN_STREAM("Position in blending path (" << i << ") invalid: Joint limit or collision detected\n");
+        return false;
+      }
+    }
+
+
     // Fill in result trajectories
     res.plan.trajectory_process = process;
     res.plan.trajectory_approach = approach;
diff --git a/godel_process_planning/src/common_utils.cpp b/godel_process_planning/src/common_utils.cpp
index 6e0e0a77..5e4bae50 100644
--- a/godel_process_planning/src/common_utils.cpp
+++ b/godel_process_planning/src/common_utils.cpp
@@ -229,28 +229,6 @@ trajectory_msgs::JointTrajectory godel_process_planning::planFreeMove(
     moveit::core::RobotModelConstPtr moveit_model, const std::vector<double>& start,
     const std::vector<double>& stop)
 {
-  // Using a mutable model, turns collision checking on for just the
-  // period of this function. Functions called in this function may
-  // throw exceptions and this makes sure the system state is always
-  // valid
-  struct CollisionsGuard
-  {
-    CollisionsGuard(descartes_core::RobotModel& model) : model_(model)
-    {
-      model.setCheckCollisions(true);
-      ROS_WARN_STREAM("Enabling collision");
-    }
-    ~CollisionsGuard()
-    {
-      model_.setCheckCollisions(false);
-      ROS_WARN_STREAM("Disable collision");
-    }
-    descartes_core::RobotModel& model_;
-  };
-
-  // Create gaurd to enable collisions only for this function
-  CollisionsGuard guard(model);
-
   // Attempt joint interpolated motion
   DescartesTraj joint_approach = createJointPath(start, stop);
 
@@ -281,8 +259,6 @@ std::vector<std::vector<double>>
 godel_process_planning::filterColliding(descartes_core::RobotModel& model,
                                         const std::vector<std::vector<double>>& candidates)
 {
-  model.setCheckCollisions(true);
-
   std::vector<std::vector<double>> results;
 
   for (const auto& c : candidates)
@@ -290,7 +266,5 @@ godel_process_planning::filterColliding(descartes_core::RobotModel& model,
     if (model.isValid(c))
       results.push_back(c);
   }
-
-  model.setCheckCollisions(false);
   return results;
 }
diff --git a/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp b/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp
index ec661e86..7b5d87ae 100644
--- a/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp
+++ b/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp
@@ -85,28 +85,9 @@ bool AbbIrb2400RobotModel::getAllIK(const Eigen::Affine3d& pose,
       ROS_DEBUG_STREAM("Original solution size: " << sol.size()
                                                   << ", number of joints: " << num_joints_);
 
-      bool obeys_limits = true;
-      for (unsigned int i = 0; i < sol.size(); i++)
-      {
-        // Add tolerance to limit check
-        if (joint_has_limits_vector_[i] &&
-            ((sol[i] < (joint_min_vector_[i] - JOINT_LIMIT_TOLERANCE)) ||
-             (sol[i] > (joint_max_vector_[i] + JOINT_LIMIT_TOLERANCE))))
-        {
-          // One element of solution is not within limits
-          obeys_limits = false;
-          ROS_DEBUG_STREAM("Not in limits! " << i << " value " << sol[i]
-                                             << " has limit: " << joint_has_limits_vector_[i]
-                                             << "  being  " << joint_min_vector_[i] << " to "
-                                             << joint_max_vector_[i]);
-          break;
-        }
-      }
-
-      if (obeys_limits)
-      {
+      if (isValid(sol))
         joint_poses.push_back(sol);
-      }
+
     }
   }
 
diff --git a/godel_robots/abb/godel_irb2400/godel_irb2400_support/launch/irb2400_blending.launch b/godel_robots/abb/godel_irb2400/godel_irb2400_support/launch/irb2400_blending.launch
index 89e7e20d..4669b7be 100644
--- a/godel_robots/abb/godel_irb2400/godel_irb2400_support/launch/irb2400_blending.launch
+++ b/godel_robots/abb/godel_irb2400/godel_irb2400_support/launch/irb2400_blending.launch
@@ -53,7 +53,7 @@
   <group unless="$(arg sim_sensor)">
     <remap from="/depth/points" to="/sensor_point_cloud"/>
     <param name="camera_frame_id" type="string" value="tool0" />
-    <node name="ensenso_publisher_node" pkg="ensenso_publisher" type="ensenso_publisher_node" output="screen">
+    <node name="ensenso_publisher_node" pkg="ensenso_publisher" type="ensenso_publisher_node" output="screen" required="true">
         <param name="FlexView" type="bool" value="true" />
         <param name="FlexViewImages" type="int" value="4" />
     </node>
diff --git a/godel_robots/abb/godel_irb2400/godel_irb2400_support/rviz/irb2400_blending.rviz b/godel_robots/abb/godel_irb2400/godel_irb2400_support/rviz/irb2400_blending.rviz
index 4ebaaaa2..3cea3dd5 100644
--- a/godel_robots/abb/godel_irb2400/godel_irb2400_support/rviz/irb2400_blending.rviz
+++ b/godel_robots/abb/godel_irb2400/godel_irb2400_support/rviz/irb2400_blending.rviz
@@ -344,6 +344,38 @@ Visualization Manager:
         {}
       Update Interval: 0
       Value: false
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.00499999989
+      Axes Radius: 0.00100000005
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: EdgePoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Axes
+      Topic: /edge_visualization
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.00499999989
+      Axes Radius: 0.00100000005
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: false
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: BlendPoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Axes
+      Topic: /blend_visualization
+      Unreliable: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -362,7 +394,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.96323609
+      Distance: 4.5955658
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -386,12 +418,12 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1176
+  Height: 752
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000001000000000000022b00000452fc0200000002fc0000002800000452000000fb0100001dfa000000020100000003fb000000240052006f0062006f00740042006c0065006e00640069006e006700500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000ffffffff0000016a00fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c0100000000000001ba0000009500fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c00000004480000001600000000000000000000040f0000045200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000001000000000000022b000002aafc0200000002fc00000028000002aa000000fb0100001dfa000000000100000003fb000000240052006f0062006f00740042006c0065006e00640069006e006700500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000ffffffff0000016a00fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c0100000000000001ba0000009500fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c000000044800000016000000000000000000000414000002aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RobotBlendingPanel:
     collapsed: false
-  Width: 1600
+  Width: 1605
   X: 0
-  Y: 24
+  Y: 28
diff --git a/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h b/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h
index c8794b97..a6fa9ca9 100644
--- a/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h
+++ b/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h
@@ -146,6 +146,7 @@ class SurfaceDetection
   void save_parameters(const std::string& filename);
 
   bool find_surfaces();
+  bool find_surfaces_old();
   std::string get_results_summary();
 
   static void mesh_to_marker(const pcl::PolygonMesh& mesh, visualization_msgs::Marker& marker);
diff --git a/godel_surface_detection/include/godel_surface_detection/detection/surface_segmentation.h b/godel_surface_detection/include/godel_surface_detection/detection/surface_segmentation.h
new file mode 100644
index 00000000..0b6d00a9
--- /dev/null
+++ b/godel_surface_detection/include/godel_surface_detection/detection/surface_segmentation.h
@@ -0,0 +1,606 @@
+#ifndef SURFACE_SEGMENTATION_H
+#define SURFACE_SEGMENTATION_H
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/surface/gp3.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/geometry/mesh_base.h>
+#include <pcl/geometry/triangle_mesh.h>
+#include <pcl/geometry/quad_mesh.h>
+#include <pcl/geometry/polygon_mesh.h>
+#include <pcl/geometry/mesh_conversion.h>
+#include <pcl/features/boundary.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/vtk_io.h>
+#include <pcl/surface/concave_hull.h>
+#include <pcl/surface/ear_clipping.h>
+#include <pcl/segmentation/region_growing.h>
+#include <pcl/filters/filter.h>
+#include <boost/foreach.hpp>
+#include <Eigen/Geometry>
+#include <Eigen/StdVector>
+
+static const int MAX_CLUSTER_SIZE = 100000;
+static const int MIN_CLUSTER_SIZE = 2500;
+static const int NUM_NEIGHBORS = 30;
+
+
+template <bool IsManifoldT>
+struct MeshTraits
+{
+  typedef pcl::PointXYZRGBNormal                       VertexData;
+  typedef pcl::geometry::NoData                        HalfEdgeData;
+  typedef pcl::geometry::NoData                        EdgeData;
+  typedef pcl::geometry::NoData                        FaceData;
+  typedef boost::integral_constant <bool, IsManifoldT> IsManifold;
+};
+
+typedef MeshTraits <true >                               ManifoldMeshTraits;
+typedef pcl::geometry::PolygonMesh <ManifoldMeshTraits>  Mesh;
+typedef typename Mesh::HalfEdgeIndex                     HalfEdgeIndex;
+typedef typename Mesh::HalfEdgeIndices                   HalfEdgeIndices;
+typedef typename Mesh::InnerHalfEdgeAroundFaceCirculator IHEAFC;
+
+
+/** @class world_background_subtraction
+@brief Maintains record of baseline sensor data to provide method to remove them leaving only new objects in the scene
+*/
+class surfaceSegmentation
+{
+  public:
+
+  // mesh results
+  pcl::PolygonMesh triangles_;
+  std::vector<int> parts_;
+  std::vector<int> states_;
+
+  // segmentation results
+  std::vector <pcl::PointIndices> clusters_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_;
+  Mesh HEM_;
+
+  // smoothing filter
+  int num_coef_;
+  std::vector<double> coef_;
+  double gain_;
+
+  // search terms
+  double radius_;
+
+  /** @brief default constructor */
+  surfaceSegmentation()
+  {
+    // initialize pointers to cloud members
+    input_cloud_= pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  }
+
+  /** @brief distructor */
+  ~surfaceSegmentation()
+  {
+    input_cloud_ =  pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    normals_ =  pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>);
+    input_cloud_->clear();
+  }
+
+  /** @brief constructor that sets the background cloud, also initializes the KdTree for searching
+  @param bg_cloud the set of points defining the background
+  */
+  surfaceSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr icloud)
+  {
+    input_cloud_ =  pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    normals_ =  pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>);
+    setInputCloud(icloud);
+    removeNans();
+    computeNormals();
+  }
+
+  /** @brief sets the background cloud, replaces whatever points exists if any
+  @param background_cloud the cloud representing the background
+  */
+  void setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr icloud)
+  {
+    input_cloud_->clear();
+    for(const auto& pt : *icloud)
+    {
+      input_cloud_->push_back(pt);
+    }
+    computeNormals();
+  }
+
+  /** @brief adds new points to the background, and reinitializes the kd_tree for searching
+  @param bg_cloud additional background points
+  */
+  void addCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr icloud)
+  {
+    // push input_cloud onto icloud and then add, this strange sequence keeps ordering of clouds
+    // and does not duplicate setInputCloud code
+    for(const auto& pt : input_cloud_->points)
+    {
+      icloud->push_back(pt);
+    }
+    setInputCloud(icloud);
+  }
+
+  /** @brief creates a cloud from every point estimated to be on the boundary of input_cloud_
+  @return a boundary point cloud
+  */
+  pcl::PointCloud<pcl::Boundary>::Ptr getBoundaryCloud()
+  {
+    pcl::PointCloud<pcl::Boundary>::Ptr bps (new pcl::PointCloud<pcl::Boundary> ());
+
+    if(normals_->points.size()==0 || input_cloud_->points.size() == 0)
+    {
+      ROS_INFO_STREAM("Must set input_cloud_ and comput normals_ before calling getBoundaryCloud()");
+    }
+    else
+    {
+      pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> best;
+      best.setInputCloud(input_cloud_);
+      best.setInputNormals(normals_);
+      best.setRadiusSearch (radius_);
+      //	best.setAngleThreshold(90.0*3.14/180.0);
+      best.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
+      best.compute(*bps);
+    }
+
+    return(bps);
+  }
+
+  std::vector <pcl::PointIndices> computeSegments(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &colored_cloud)
+  {
+    // Region growing
+    pcl::search::Search<pcl::PointXYZ>::Ptr tree =
+        boost::shared_ptr<pcl::search::Search<pcl::PointXYZ>> (new pcl::search::KdTree<pcl::PointXYZ>);
+    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
+
+    rg.setSmoothModeFlag (false); // Depends on the cloud being processed
+    rg.setSmoothnessThreshold (10.0 / 180.0 * M_PI);
+    rg.setMaxClusterSize(MAX_CLUSTER_SIZE);
+    rg.setSearchMethod (tree);
+    rg.setMinClusterSize(MIN_CLUSTER_SIZE);
+    rg.setNumberOfNeighbours (NUM_NEIGHBORS);
+
+    float smooth_thresh = rg.getSmoothnessThreshold();
+    float resid_thresh = rg.getResidualThreshold();
+
+    // rg.setCurvatureTestFlag();
+    rg.setResidualTestFlag(true);
+    rg.setResidualThreshold(resid_thresh);
+    rg.setCurvatureThreshold(1.0);
+    rg.setInputCloud (input_cloud_);
+    rg.setInputNormals (normals_);
+
+    rg.extract (clusters_);
+    colored_cloud = rg.getColoredCloud();
+    return(clusters_);
+  }
+
+
+  /** @brief computes mesh on the cloud results are in triangles_, parts_, and states_ */
+  Mesh computeMesh()
+  {
+    // use gpg3 to create a mesh, then traverse boundary of mesh to get boundaries
+    // concatenate rgb fields and normals to cloud
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_with_colors(new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+    for(const auto& pt : input_cloud_->points)
+    {
+      pcl::PointXYZRGB npt(0,255,0);
+      npt.x = pt.x;
+      npt.y = pt.y;
+      npt.z = pt.z;
+      cloud_with_colors->push_back(npt);
+    }
+    pcl::concatenateFields (*cloud_with_colors, *normals_, *cloud_with_normals);
+
+    // Initialize objects
+    pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
+
+    // Create search tree*
+    pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
+    tree->setInputCloud (cloud_with_normals);
+
+    // Set typical values for the parameters
+    gp3.setSearchRadius (radius_);
+    gp3.setMu (2.5);
+    gp3.setMaximumNearestNeighbors (500);
+    gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
+    gp3.setMinimumAngle(M_PI/18); // 10 degrees
+    gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
+    gp3.setNormalConsistency(false);
+    gp3.setSearchMethod (tree);
+    gp3.setInputCloud (cloud_with_normals);
+
+    // Get results
+    gp3.reconstruct (triangles_);
+    parts_ = gp3.getPartIDs();
+    states_ = gp3.getPointStates();
+
+    pcl::console::print_highlight ("convert to half edge mesh\n");
+    pcl::geometry::toHalfEdgeMesh(triangles_, HEM_);
+
+    return(HEM_);
+  }
+
+    std::pair<int, int> getNextUnused(std::vector< std::pair<int,int> > used)
+    {
+      std::pair<int,int> rtn;
+      rtn.first=-1;
+      rtn.second=-1;
+      for(int i=0;i<used.size();i++)
+      {
+        if(used[i].first == 0)
+        {
+        rtn = used[i];
+        used[i].first = 1;
+        break;
+        }
+      }
+      return(rtn);
+    }
+
+  int sortBoundary(pcl::IndicesPtr& boundary_indices, std::vector<pcl::IndicesPtr> &sorted_boundaries)
+  {
+    int longest = 0;
+    int long_idx = 0;
+    int length=0;
+
+    for(int i = 0; i < sorted_boundaries.size(); i++)
+      sorted_boundaries[i]->clear();
+
+    sorted_boundaries.clear();
+
+
+    /* initialize used pairs */
+    std::vector<std::pair<int,int> > used;
+    used.reserve(boundary_indices->size());
+    for(int i = 0; i < boundary_indices->size(); i++)
+    {
+      std::pair<int, int> p;
+      p.first = 0;
+      p.second = boundary_indices->at(i);
+      used.push_back(p);
+    }
+
+    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree(true);// true indicates return sorted radius search results
+    kdtree.setInputCloud(input_cloud_, boundary_indices); // use just the boundary points for searching
+
+    std::pair<int, int> n = getNextUnused(used);
+    while( n.first >= 0 )
+    {
+      // add first point to the current boundary
+      pcl::IndicesPtr current_boundary(new std::vector<int>);
+      current_boundary->push_back(n.second);
+
+      // find all points within small radius of current boundary point
+      std::vector<int> pt_indices;
+      std::vector<float> pt_dist;
+      pcl::PointXYZ spt = input_cloud_->points[n.second];
+
+      while ( kdtree.radiusSearch (spt, radius_, pt_indices, pt_dist) > 1 )
+      { // gives index into input_cloud_,
+        int q = 0;
+        int add_pt_idx = -1;
+        do
+        {
+          // find closest unused point in vicinity
+          int pair_index=0;
+
+          // for each item in used list
+          for(int i=0;i<used.size();i++)
+          {
+            // look for a match
+            if(pt_indices[q] == used[i].second)
+            {
+              // see if match is used
+              if(used[i].first ==0)
+              {
+                pair_index= i;
+                used[i].first = 1; // mark it used
+                add_pt_idx = used[i].second;
+              }// used
+              break;
+            }// match indices
+          }
+
+          q++;
+
+        } while (q < pt_indices.size() && add_pt_idx == -1); // unused point in the vicinity of the current spt?
+
+        if(add_pt_idx !=-1)
+        {
+          current_boundary->push_back(add_pt_idx);
+          spt = input_cloud_->points[add_pt_idx]; // search near the new point next time
+        }
+        else
+        { // end if ad_pt_idx was found
+          break;		/* end of boundary */
+        }
+      }// there are points within the radius
+
+      sorted_boundaries.push_back(current_boundary);
+      n = getNextUnused(used);
+    }
+
+    return(sorted_boundaries.size());
+  }
+
+
+  void setSearchRadius(double radius)
+  {
+    if(radius > 0)
+      radius_ = radius;
+  }
+
+
+  double  getSearchRadius()
+  {
+    return(radius_);
+  }
+
+
+  bool setSmoothCoef(std::vector<double> &coef)
+  {
+    if(coef.size() % 2 == 1)
+    {		// smoothing filters must have an odd number of coefficients
+      coef_.clear();
+      num_coef_ = coef.size();
+      double sum = 0;
+      for(int i = 0; i < num_coef_; i++)
+      {
+        coef_.push_back(coef[i]);
+        sum += coef[i];
+      }
+      gain_ = sum;		// set gain to be the sum of the coefficients because we need unity gain
+      return(true);
+    }
+    else
+    {
+      return(false);
+    }
+  }
+
+
+  void smoothVector(std::vector<double>&x_in, std::vector<double> &x_out)
+  {
+    int n = x_in.size();
+    if( n <= num_coef_)
+    {
+      x_out = x_in;
+    }
+    else
+    {
+      // initialize the filter using tail of x_in
+      std::vector<double> xv;
+      xv.clear();
+      for(int j = num_coef_ - 1; j >= 0; j--)
+        xv.push_back(x_in[n-j-1]);
+
+      // cycle through every and apply the filter
+      for(int j = 1; j < n - 1; j++)
+      {
+        // shift backwards
+        for(int k = 0; k < num_coef_ - 1; k++)
+          xv[k] = xv[k + 1];
+
+        // get next input to filter which is num_coef/2 in front of current point being smoothed
+        int idx = (j + num_coef_ / 2) % n;
+        xv[num_coef_ - 1] = x_in[idx]; // j'th point
+
+        // apply the filter
+        double sum = 0.0;
+        for(int k = 0; k < num_coef_; k++)
+          sum += xv[k] * coef_[k];
+
+        // save point
+        x_out.push_back(sum / gain_);
+      }// end for every point
+    }
+  }
+
+
+  void smoothPointNormal(std::vector<pcl::PointNormal> &pts_in,
+                         std::vector<pcl::PointNormal> &pts_out)
+  {
+    std::vector<double> x_in, x_out, y_in, y_out, z_in, z_out;
+    std::vector<double> nx_in, nx_out, ny_in, ny_out, nz_in, nz_out;
+
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      x_in.push_back(pts_in[i].x);
+      y_in.push_back(pts_in[i].y);
+      z_in.push_back(pts_in[i].z);
+      nx_in.push_back(pts_in[i].normal_x);
+      ny_in.push_back(pts_in[i].normal_y);
+      nz_in.push_back(pts_in[i].normal_z);
+    }
+    smoothVector(x_in,x_out);
+    smoothVector(y_in,y_out);
+    smoothVector(z_in,z_out);
+    smoothVector(nx_in,nx_out);
+    smoothVector(ny_in,ny_out);
+    smoothVector(nz_in,nz_out);
+
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      pcl::PointNormal pt;
+      pt.x = x_out[i];
+      pt.y = y_out[i];
+      pt.z = z_out[i];
+      double norm = sqrt(nx_out[i] * nx_out[i] + ny_out[i] * ny_out[i] + nz_out[i] * nz_out[i]);
+      pt.normal_x = nx_out[i] / norm;
+      pt.normal_y = ny_out[i] / norm;
+      pt.normal_z = nz_out[i] / norm;
+      pts_out.push_back(pt);
+    }
+  }
+
+
+  void getBoundaryTrajectory(std::vector<pcl::IndicesPtr> &boundaries,
+                             int sb,
+                             std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> &poses)
+  {
+    // grab the position and normal values
+    std::vector<pcl::PointNormal> pts, spts;
+    for(int i = 0; i < boundaries[sb]->size(); i++)
+    {
+      pcl::PointNormal pt;
+      int idx = boundaries[sb]->at(i);
+      pt.x = input_cloud_->points[idx].x;
+      pt.y = input_cloud_->points[idx].y;
+      pt.z = input_cloud_->points[idx].z;
+
+      int sign_ofz=1;
+      if(normals_->at(idx).normal_z < 0)
+        sign_ofz = -1;
+
+      pt.normal_x = sign_ofz * normals_->at(idx).normal_x;
+      pt.normal_y = sign_ofz * normals_->at(idx).normal_y;
+      pt.normal_z = sign_ofz * normals_->at(idx).normal_z;
+      pts.push_back(pt);
+    }
+    smoothPointNormal(pts, spts);
+
+    std::vector<pcl::PointXYZ> vels;
+    for(int i = 0; i < spts.size(); i++)
+    {
+      pcl::PointXYZ v;
+      int next = (i + 1) % pts.size();
+      v.x = spts[next].x - spts[i].x;
+      v.y = spts[next].y - spts[i].y;
+      v.z = spts[next].z - spts[i].z;
+      double norm = sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
+      if(norm == 0)
+        norm = 1.0;	/* avoid division by zero */
+      v.x = v.x / norm;
+      v.y = v.y / norm;
+      v.z = v.z / norm;
+      vels.push_back(v);
+    }
+
+    // make sure normal and velocity vectors are orthogonal
+    for(int i = 0; i < spts.size(); i++)
+    {
+       double dot = spts[i].normal_x * vels[i].x + spts[i].normal_y * vels[i].y + spts[i].normal_z * vels[i].z;
+       vels[i].x -= dot * spts[i].normal_x;
+       vels[i].y -= dot * spts[i].normal_y;
+       vels[i].z -= dot * spts[i].normal_z;
+
+       double norm = sqrt(vels[i].x * vels[i].x + vels[i].y * vels[i].y + vels[i].z * vels[i].z);
+
+       if(norm == 0)
+         norm = 1;
+
+       vels[i].x = vels[i].x / norm;
+       vels[i].y = vels[i].y / norm;
+       vels[i].z = vels[i].z / norm;
+
+    }
+
+    poses.clear();
+    for(int i = 0; i < spts.size(); i++)
+    {
+      Eigen::Matrix4d current_pose = Eigen::MatrixXd::Identity(4, 4);
+      current_pose(0, 3) = spts[i].x;
+      current_pose(1, 3) = spts[i].y;
+      current_pose(2, 3) = spts[i].z;
+
+      // set z vector same as of normal of surface
+      current_pose(0, 2) = spts[i].normal_x;
+      current_pose(1, 2) = spts[i].normal_y;
+      current_pose(2, 2) = spts[i].normal_z;
+
+      // set x of tool in direction of motion
+      current_pose(0, 0) = vels[i].x;
+      current_pose(1, 0) = vels[i].y;
+      current_pose(2, 0) = vels[i].z;
+
+      // y is the cross product of z with x
+      current_pose(0, 1) =  current_pose(1, 2) * current_pose(2,0) - current_pose(1, 0) * current_pose(2, 2);
+      current_pose(1, 1) = -current_pose(0, 2) * current_pose(2,0) + current_pose(0, 0) * current_pose(2, 2);
+      current_pose(2, 1) =  current_pose(0, 2) * current_pose(1,0) - current_pose(0, 0) * current_pose(1, 2);
+      poses.push_back(current_pose);
+    }
+  }
+
+
+  bool applyConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr& in, pcl::PolygonMesh& mesh)
+  {
+    pcl::PolygonMesh::Ptr hull_mesh_ptr(new pcl::PolygonMesh);
+    pcl::ConcaveHull<pcl::PointXYZ> chull;
+    chull.setInputCloud(in);
+    chull.setAlpha(500.0);
+    chull.reconstruct(mesh);
+
+    return mesh.polygons.size() > 0;
+  }
+
+
+  void getBoundBoundaryHalfEdges (const Mesh &mesh,
+                                  std::vector <Mesh::HalfEdgeIndices>& boundary_he_collection,
+                                  const size_t  expected_size = 3)
+  {
+
+    boundary_he_collection.clear ();
+
+    HalfEdgeIndices boundary_he; boundary_he.reserve (expected_size);
+    std::vector <bool> visited (mesh.sizeEdges (), false);
+    IHEAFC circ, circ_end;
+
+    for (HalfEdgeIndex i (0); i<HalfEdgeIndex (mesh.sizeHalfEdges ()); ++i)
+    {
+      if (mesh.isBoundary (i) && !visited [pcl::geometry::toEdgeIndex (i).get ()])
+      {
+        boundary_he.clear ();
+
+        circ     = mesh.getInnerHalfEdgeAroundFaceCirculator (i);
+        circ_end = circ;
+        do
+        {
+        visited [pcl::geometry::toEdgeIndex (circ.getTargetIndex ()).get ()] = true;
+        boundary_he.push_back (circ.getTargetIndex ());
+        } while ( ++circ != circ_end);
+
+        boundary_he_collection.push_back (boundary_he);
+      }
+    }
+  }
+
+
+
+private:
+  /** @brief remove any NAN points, otherwise many algorityms fail */
+  void removeNans()
+  {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr nonans_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    nonans_cloud->is_dense = false;
+    std::vector<int> indices;
+    pcl::removeNaNFromPointCloud (*input_cloud_, *nonans_cloud, indices);
+    setInputCloud(nonans_cloud);
+  }
+
+
+  /** @brief compute the normals and store in normals_, this is requried for both segmentation and meshing*/
+  void computeNormals()
+  {
+    normals_->points.clear();
+    // Estimate the normals
+    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
+    ne.setInputCloud (input_cloud_);
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
+    ne.setSearchMethod (tree);
+    //  ne.setRadiusSearch (radius_);
+    ne.setKSearch (100);
+    ne.compute (*normals_);
+  }
+
+  pcl::PointCloud<pcl::Normal>::Ptr normals_;
+  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals_;
+
+};
+#endif
diff --git a/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h b/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h
index e2b39b7b..ae33b64d 100644
--- a/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h
+++ b/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h
@@ -18,6 +18,7 @@
 
 #include <godel_surface_detection/scan/robot_scan.h>
 #include <godel_surface_detection/detection/surface_detection.h>
+#include <godel_surface_detection/detection/surface_segmentation.h>
 #include <godel_surface_detection/interactive/interactive_surface_server.h>
 
 #include <godel_msgs/SurfaceDetection.h>
@@ -153,13 +154,16 @@ class SurfaceBlendingService
                        visualization_msgs::Marker& visualization,
                        geometry_msgs::PoseArray& path);
 
-  bool requestEdgePath(const godel_process_path::PolygonBoundaryCollection& boundaries,
-                       const geometry_msgs::Pose& boundary_pose,
-                       const double z_ref,
+
+  bool requestEdgePath(std::vector<pcl::IndicesPtr>& boundaries,
+                       int index,
+                       surfaceSegmentation& SS,
                        visualization_msgs::Marker& visualization,
                        geometry_msgs::PoseArray& path);
 
-  ProcessPathResult generateProcessPath(const std::string& name, const pcl::PolygonMesh& mesh,
+  ProcessPathResult generateProcessPath(const std::string& name,
+                                        const pcl::PolygonMesh& mesh,
+                                        const godel_surface_detection::detection::Cloud::Ptr,
                                         const godel_msgs::BlendingPlanParameters& params,
                                         const godel_msgs::ScanPlanParameters& scan_params);
 
@@ -208,6 +212,9 @@ class SurfaceBlendingService
   ros::Publisher selected_surf_changed_pub_;
   ros::Publisher point_cloud_pub_;
   ros::Publisher tool_path_markers_pub_;
+  ros::Publisher blend_visualization_pub_;
+  ros::Publisher edge_visualization_pub_;
+
   // Timers
   bool stop_tool_animation_;
 
diff --git a/godel_surface_detection/launch/godel_core.launch b/godel_surface_detection/launch/godel_core.launch
index 1a5ad102..8aa5b388 100644
--- a/godel_surface_detection/launch/godel_core.launch
+++ b/godel_surface_detection/launch/godel_core.launch
@@ -4,7 +4,7 @@
   <arg name="config_path"/>
 
     <!-- surface detection service -->
-  <node name="surface_blending_service" pkg="godel_surface_detection" type="surface_blending_service" output="screen">
+  <node name="surface_blending_service" pkg="godel_surface_detection" type="surface_blending_service" output="screen" required="true">
     <rosparam command="load" file="$(arg config_path)/robot_scan.yaml"/>
     <rosparam command="load" file="$(arg config_path)/blending_plan.yaml"/>
     <rosparam command="load" file="$(arg config_path)/surface_detection.yaml"/>
diff --git a/godel_surface_detection/src/detection/surface_detection.cpp b/godel_surface_detection/src/detection/surface_detection.cpp
index e1370306..8e035d5e 100644
--- a/godel_surface_detection/src/detection/surface_detection.cpp
+++ b/godel_surface_detection/src/detection/surface_detection.cpp
@@ -15,6 +15,7 @@
 */
 
 #include <godel_surface_detection/detection/surface_detection.h>
+#include <godel_surface_detection/detection/surface_segmentation.h>
 #include <pcl_ros/transforms.h>
 #include <tf/transform_listener.h>
 #include <pcl/filters/statistical_outlier_removal.h>
@@ -42,6 +43,7 @@
 const static double CONCAVE_HULL_ALPHA = 0.1;
 const static double PASSTHROUGH_Z_MIN = -5;
 const static double PASSTHROUGH_Z_MAX = 5;
+const static int DOWNSAMPLE_NUMBER = 3;
 
 namespace godel_surface_detection
 {
@@ -268,7 +270,108 @@ void SurfaceDetection::get_region_colored_cloud(sensor_msgs::PointCloud2& cloud_
   cloud_msg.header.frame_id = params_.frame_id;
 }
 
+
 bool SurfaceDetection::find_surfaces()
+{
+  ROS_INFO_STREAM("Find Surfaces Call");
+
+  // Reset members
+  surface_clouds_.clear();
+  mesh_markers_.markers.clear();
+  meshes_.clear();
+
+  // Ensure cloud ptr is not empty
+  if (full_cloud_ptr_->empty())
+    return false;
+
+  // Create Processing Cloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr process_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
+  process_cloud_ptr->header = full_cloud_ptr_->header;
+
+  // Subsample full cloud into processing cloud
+  for(const auto& pt : full_cloud_ptr_->points)
+  {
+    int q = rand() % DOWNSAMPLE_NUMBER;
+    if (q ==0)
+    {
+      if(pt.x !=0.0 && pt.y!=0.0 && pt.z !=0.0 && pcl::isFinite(pt))
+      {
+        process_cloud_ptr->push_back(pt);
+      }
+    }
+  }
+
+  // Segment the part into surface clusters using a "region growing" scheme
+  surfaceSegmentation SS(process_cloud_ptr);
+  region_colored_cloud_ptr_ = CloudRGB::Ptr(new CloudRGB());
+  std::vector <pcl::PointIndices> clusters = SS.computeSegments(region_colored_cloud_ptr_);
+  pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices());
+
+  // Extract points from clusters into their own point clouds
+  for (int i = 0; i < clusters.size(); i++)
+  {
+    Cloud::Ptr segment_cloud_ptr = Cloud::Ptr(new Cloud());
+    pcl::PointIndices& indices = clusters[i];
+    if (indices.indices.size() == 0)
+      continue;
+
+    if (indices.indices.size() >= params_.rg_min_cluster_size)
+    {
+      inliers_ptr->indices.insert(inliers_ptr->indices.end(), indices.indices.begin(), indices.indices.end());
+
+      pcl::copyPointCloud(*process_cloud_ptr, indices, *segment_cloud_ptr);
+      surface_clouds_.push_back(segment_cloud_ptr);
+    }
+  }
+
+  // Remove largest cluster if appropriate
+  if (params_.ignore_largest_cluster && surface_clouds_.size() > 1)
+  {
+    int largest_index = 0;
+    int largest_size = 0;
+    for (int i = 0; i < surface_clouds_.size(); i++)
+    {
+      if (surface_clouds_[i]->points.size() > largest_size)
+      {
+        largest_size = surface_clouds_[i]->points.size();
+        largest_index = i;
+      }
+    }
+    surface_clouds_.erase(surface_clouds_.begin() + largest_index);
+  }
+
+  // Compute mesh from point clouds
+  for (int i = 0; i < surface_clouds_.size(); i++)
+  {
+    pcl::PolygonMesh mesh;
+    visualization_msgs::Marker marker;
+
+    if (apply_concave_hull(*surface_clouds_[i], mesh))
+    {
+      // Create marker from mesh
+      mesh_to_marker(mesh, marker);
+
+      // saving other properties
+      marker.header.frame_id = mesh.header.frame_id = surface_clouds_[i]->header.frame_id;
+      marker.id = i;
+      marker.color.a = params_.marker_alpha;
+
+      // Push marker to mesh_markers_
+      ROS_INFO_STREAM("Adding a marker for mesh with " + std::to_string(marker.points.size()) + " points");
+      mesh_markers_.markers.push_back(marker);
+
+      // Push mesh to meshes_
+      meshes_.push_back(mesh);
+    }
+    else
+      ROS_INFO_STREAM("Apply concave hull failed");
+  }
+
+  return true;
+}
+
+
+bool SurfaceDetection::find_surfaces_old()
 {
   // main process point cloud
   Cloud::Ptr process_cloud_ptr = Cloud::Ptr(new Cloud());
@@ -466,7 +569,7 @@ bool SurfaceDetection::find_surfaces()
       }
     }
 
-    ROS_INFO_STREAM("Removing larges cluster from results: cluster index [ "
+    ROS_INFO_STREAM("Removing largest cluster from results: cluster index [ "
                     << largest_index << " ], cluster size [ " << largest_size << " ]");
     surface_clouds_.erase(surface_clouds_.begin() + largest_index);
     segment_normals.erase(segment_normals.begin() + largest_index);
@@ -495,6 +598,7 @@ bool SurfaceDetection::find_surfaces()
       marker.color.a = params_.marker_alpha;
       mesh_markers_.markers.push_back(marker);
       meshes_.push_back(mesh);
+      ROS_INFO_STREAM(marker);
 
       ROS_INFO_STREAM("Triangulation for surface " << i << " completed");
     }
diff --git a/godel_surface_detection/src/services/blending_service_path_generation.cpp b/godel_surface_detection/src/services/blending_service_path_generation.cpp
index e4958dcb..1594f98d 100644
--- a/godel_surface_detection/src/services/blending_service_path_generation.cpp
+++ b/godel_surface_detection/src/services/blending_service_path_generation.cpp
@@ -1,4 +1,8 @@
 #include <godel_surface_detection/services/surface_blending_service.h>
+#include <godel_surface_detection/detection/surface_segmentation.h>
+#include <godel_surface_detection/detection/surface_detection.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/PointIndices.h>
 
 // Temporary constants for storing blending path `planning parameters
 // Will be replaced by loadable, savable parameters
@@ -21,11 +25,17 @@ const static double SCAN_TRAJECTORY_ANGLE_DISC = 0.2;
 const static int SCAN_APPROACH_STEP_COUNT = 5;
 const static double SCAN_APPROACH_STEP_DISTANCE = 0.01; // 5cm
 
+// Edge Processing constants
+const static double SEGMENTATION_SEARCH_RADIUS = 0.030; // 3cm
+const static int BOUNDARY_THRESHOLD = 10;
+
 // Variables to select path type
 const static int PATH_TYPE_BLENDING = 0;
 const static int PATH_TYPE_SCAN = 1;
 const static int PATH_TYPE_EDGE = 2;
 
+const static std::string SURFACE_DESIGNATION = "surface_marker_server_";
+
 /**
  * Prototype ProcessPlan refactoring - make it compatible with trajectory library and GUI
  */
@@ -34,11 +44,12 @@ filterPolygonBoundaries(const godel_process_path::PolygonBoundaryCollection& bou
                         const godel_msgs::BlendingPlanParameters& params)
 {
   godel_process_path::PolygonBoundaryCollection filtered_boundaries;
+
   for (std::size_t i = 0; i < boundaries.size(); ++i)
   {
     const godel_process_path::PolygonBoundary& bnd = boundaries[i];
-
     double circ = godel_process_path::polygon_utils::circumference(bnd);
+
     if (circ < params.min_boundary_length)
     {
       ROS_WARN_STREAM("Ignoring boundary with length " << circ);
@@ -57,83 +68,28 @@ filterPolygonBoundaries(const godel_process_path::PolygonBoundaryCollection& bou
   return filtered_boundaries;
 }
 
-bool SurfaceBlendingService::requestEdgePath(
-    const godel_process_path::PolygonBoundaryCollection& boundaries,
-    const geometry_msgs::Pose& boundary_pose,
-    const double z_ref,
-    visualization_msgs::Marker& visualization,
-    geometry_msgs::PoseArray& path)
-{
-  // Construct Visualization
-  geometry_msgs::Point pnt;
-  size_t idx(0);
-  for (godel_process_path::PolygonBoundaryCollection::const_iterator pb = boundaries.begin(), pb_end = boundaries.end(); pb != pb_end;
-       ++pb, ++idx)
-  {
-    for (godel_process_path::PolygonBoundary::const_iterator pt = pb->begin(), pt_end = pb->end(); pt != pt_end; ++pt)
-    {
-      pnt.x = pt->x;
-      pnt.y = pt->y;
-      pnt.z = z_ref;
-      visualization.points.push_back(pnt);
-    }
-  }
-
-  visualization.points.push_back(*(visualization.points.begin()));
 
-  // Add in an approach and depart vector
-  const geometry_msgs::Point& start_pt = visualization.points.front();
-  const geometry_msgs::Point& end_pt = visualization.points.back();
-  // Approach vector
-  std::vector<geometry_msgs::Point> approach_points;
-  for (std::size_t i = 0; i < SCAN_APPROACH_STEP_COUNT; ++i)
-  {
-    geometry_msgs::Point pt = start_pt;
-    pt.z += (SCAN_APPROACH_STEP_COUNT - i) * SCAN_APPROACH_STEP_DISTANCE;
-    approach_points.push_back(pt);
-  }
-  // Depart vector
-  std::vector<geometry_msgs::Point> depart_points;
-  for (std::size_t i = 0; i < SCAN_APPROACH_STEP_COUNT; ++i)
-  {
-    geometry_msgs::Point pt = end_pt;
-    pt.z += i * SCAN_APPROACH_STEP_DISTANCE;
-    depart_points.push_back(pt);
-  }
-  // Insert into path
-  visualization.points.insert(visualization.points.end(), depart_points.begin(), depart_points.end());
-  visualization.points.insert(visualization.points.begin(), approach_points.begin(), approach_points.end());
-
-  // Construct Poses
-  geometry_msgs::Pose p;
-  p.orientation.x = 0.0;
-  p.orientation.y = 0.0;
-  p.orientation.z = 0.0;
-  p.orientation.w = 1.0;
+bool SurfaceBlendingService::requestEdgePath(std::vector<pcl::IndicesPtr> &boundaries,
+                                             int index,
+                                             surfaceSegmentation& SS,
+                                             visualization_msgs::Marker& visualization,
+                                             geometry_msgs::PoseArray& path)
+{
+  geometry_msgs::Pose geo_pose;
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> poses;
 
-  // Transform points to world frame and generate pose
-  Eigen::Affine3d boundary_pose_eigen;
-  Eigen::Affine3d eigen_p;
-  Eigen::Affine3d result;
+  // Get boundary trajectory and trim last two poses (last poses are susceptible to large velocity changes)
+  SS.getBoundaryTrajectory(boundaries, index, poses);
+  poses.resize(poses.size() - 2);
 
-  tf::poseMsgToEigen(boundary_pose, boundary_pose_eigen);
-
-  for(int i = 0; i < visualization.points.size(); i++)
+  // Convert eigen poses to geometry poses for messaging and visualization
+  for(const auto& p : poses)
   {
-    p.position.x = visualization.points[i].x;
-    p.position.y = visualization.points[i].y;
-    p.position.z = visualization.points[i].z;
-
-    tf::poseMsgToEigen(p, eigen_p);
-    result = boundary_pose_eigen*eigen_p;
-    tf::poseEigenToMsg(result, p);
-    path.poses.push_back(p);
+    Eigen::Affine3d pose(p.matrix());
+    tf::poseEigenToMsg(pose, geo_pose);
+    path.poses.push_back(geo_pose);
   }
 
-  // blend process path calculations suceeded. Save data into results.
-  visualization.ns = PATH_NAMESPACE;
-  visualization.pose = boundary_pose;
-
   return true;
 }
 
@@ -179,6 +135,7 @@ bool SurfaceBlendingService::requestBlendPath(
     tf::poseMsgToEigen(p, eigen_p);
     result = boundary_pose_eigen*eigen_p;
     tf::poseEigenToMsg(result, p);
+    p.orientation = boundary_pose.orientation;
     path.poses.push_back(p);
   }
 
@@ -258,9 +215,63 @@ bool SurfaceBlendingService::requestScanPath(
   return true;
 }
 
+void computeBoundaries(const godel_surface_detection::detection::Cloud::Ptr surface_cloud,
+                       surfaceSegmentation& SS,
+                       std::vector< pcl::IndicesPtr>& sorted_boundaries)
+{
+  pcl::PointCloud<pcl::Boundary>::Ptr boundary_ptr = SS.getBoundaryCloud();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr boundary_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+  int k=0;
+
+  pcl::IndicesPtr boundary_idx(new std::vector<int>());
+  for(const auto& pt : boundary_ptr->points)
+  {
+    if(pt.boundary_point)
+    {
+      boundary_cloud_ptr->points.push_back(surface_cloud->points[k]);
+      boundary_idx->push_back(k);
+    }
+    k++;
+  }
+
+  ROS_INFO_STREAM("Cloud has " + std::to_string(boundary_cloud_ptr->points.size()) + " boundary points\n");
+  boundary_cloud_ptr->width = 1;
+  boundary_cloud_ptr->height = boundary_cloud_ptr->points.size();
+
+
+  // sort the boundaries
+  int num_boundaries = SS.sortBoundary(boundary_idx, sorted_boundaries);
+  ROS_INFO_STREAM("Cloud has " + std::to_string(num_boundaries)  + " boundaries");
+
+  int max=0;
+  int max_idx=0;
+
+  for(int i=0;i<sorted_boundaries.size();i++)
+  {
+    if(sorted_boundaries[i]->size() > max)
+    {
+      max = sorted_boundaries[i]->size();
+      max_idx = i;
+    }
+  }
+}
+
+void extractBoundaryCloud(const godel_surface_detection::detection::Cloud::Ptr surface_cloud,
+                          const pcl::IndicesPtr boundary_indices,
+                          godel_surface_detection::detection::Cloud::Ptr& boundary_cloud)
+{
+  // Create the filtering object
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+  extract.setInputCloud(surface_cloud);
+  extract.setIndices(boundary_indices);
+  extract.setNegative(false);
+  extract.filter(*boundary_cloud);
+}
+
 ProcessPathResult
 SurfaceBlendingService::generateProcessPath(const std::string& name,
                                             const pcl::PolygonMesh& mesh,
+                                            godel_surface_detection::detection::Cloud::Ptr surface,
                                             const godel_msgs::BlendingPlanParameters& blend_params,
                                             const godel_msgs::ScanPlanParameters& scan_params)
 {
@@ -279,11 +290,10 @@ SurfaceBlendingService::generateProcessPath(const std::string& name,
   // Read & filter boundaries that are ill-formed or too small
   PolygonBoundaryCollection filtered_boundaries =
       filterPolygonBoundaries(mesh_importer_.getBoundaries(), blend_params);
+
   // Read pose
   geometry_msgs::Pose boundary_pose;
   mesh_importer_.getPose(boundary_pose);
-  ROS_INFO_STREAM("\n\n\n\n");
-  ROS_INFO_STREAM(boundary_pose);
 
   // Send request to blend path generation service
   visualization_msgs::Marker blend_visualization;
@@ -321,44 +331,77 @@ SurfaceBlendingService::generateProcessPath(const std::string& name,
     ROS_WARN_STREAM("Could not calculate blend path for surface: " << name);
   }
 
-  // Send request to edge path generation service
-  visualization_msgs::Marker edge_visualization;
-  geometry_msgs::PoseArray edge_poses;
+  // Publish all poses for all boundaries
+  blend_poses.header.frame_id = "world_frame";
+  blend_poses.header.stamp = ros::Time::now();
+  blend_visualization_pub_.publish(blend_poses);
 
-  if (requestEdgePath(filtered_boundaries, boundary_pose, blend_visualization.points.at(10).z, edge_visualization, edge_poses))
+  ROS_INFO_STREAM("Blend Path Generation Complete");
+
+  // Send request to edge path generation service
+  std::vector<pcl::IndicesPtr> sorted_boundaries;
+
+  ROS_INFO_STREAM("Surface has " + std::to_string(surface->points.size()) + "points");
+  // Compute the boundary
+  surfaceSegmentation SS(surface);
+
+  SS.setSearchRadius(SEGMENTATION_SEARCH_RADIUS);
+  std::vector<double> filt_coef;
+  filt_coef.push_back(1);
+  filt_coef.push_back(2);
+  filt_coef.push_back(3);
+  filt_coef.push_back(4);
+  filt_coef.push_back(5);
+  filt_coef.push_back(4);
+  filt_coef.push_back(3);
+  filt_coef.push_back(2);
+  filt_coef.push_back(1);
+
+  SS.setSmoothCoef(filt_coef);
+  computeBoundaries(surface, SS, sorted_boundaries);
+
+  ROS_INFO_STREAM("Boundaries Computed");
+  geometry_msgs::PoseArray all_edge_poses;
+  for(int i = 0; i < sorted_boundaries.size(); i++)
   {
-    ProcessPathResult::value_type edge_path_result; // pair<string, viz_msgs::Marker>
-    edge_path_result.first = name + "_edge";
-    edge_path_result.second = edge_poses;
-    result.paths.push_back(edge_path_result);
+    if(sorted_boundaries.at(i)->size() < BOUNDARY_THRESHOLD)
+      continue;
 
-    // Hack for visualization sake
-    edge_visualization.header.frame_id = "world_frame";
-    edge_visualization.id = marker_counter_++;
-    edge_visualization.header.stamp = ros::Time::now();
-    edge_visualization.lifetime = ros::Duration(0.0);
-    edge_visualization.ns = "edge_path";
-    edge_visualization.pose = boundary_pose;
-    edge_visualization.action = visualization_msgs::Marker::ADD;
-    edge_visualization.type = visualization_msgs::Marker::LINE_STRIP;
-    edge_visualization.scale.x = 0.004;
-    std_msgs::ColorRGBA color;
-    color.r = 0.0;
-    color.b = 1.0;
-    color.g = 0.0;
-    color.a = 1.0;
-    edge_visualization.colors.clear();
-    edge_visualization.color = color;
+    geometry_msgs::PoseArray edge_poses;
+    visualization_msgs::Marker edge_visualization;
 
-    process_path_results_.edge_visualization_.markers.push_back(edge_visualization);
+    if(requestEdgePath(sorted_boundaries, i, SS, edge_visualization, edge_poses))
+    {
+      ProcessPathResult::value_type edge_path_result; // pair<string, geometry_msgs::PoseArray>
+      edge_path_result.first = name + "_edge_" + std::to_string(i);
 
-  }
-  else
-  {
-    // Blend path request failed
-    ROS_WARN_STREAM("Could not calculate blend path for surface: " << name);
+      /*
+       * Set the orientation for all edge points to be the orientation of the surface normal
+       * This is a hack that should be removed when planar surfaces assumption is dropped
+       * The main purpose here is to "smooth" the trajectory of the edges w.r.t. the z axis
+      */
+      for(auto& p : edge_poses.poses)
+        p.orientation = boundary_pose.orientation;
+
+      edge_path_result.second = edge_poses;
+
+      result.paths.push_back(edge_path_result);
+
+      all_edge_poses.poses.insert(std::end(all_edge_poses.poses), std::begin(edge_poses.poses), std::end(edge_poses.poses));
+    }
+    else
+    {
+      // Blend path request failed
+      ROS_WARN_STREAM("Could not calculate blend path for surface: " << name);
+    }
   }
 
+  // Publish all poses for all boundaries
+  all_edge_poses.header.frame_id = "world_frame";
+  all_edge_poses.header.stamp = ros::Time::now();
+  edge_visualization_pub_.publish(all_edge_poses);
+
+
   // Request laser scan paths
   visualization_msgs::Marker scan_visualization;
   geometry_msgs::PoseArray scan_poses;
@@ -405,6 +448,17 @@ godel_surface_detection::TrajectoryLibrary SurfaceBlendingService::generateMotio
   surface_server_.get_selected_surfaces(meshes);
   std::vector<std::string> names;
   surface_server_.get_selected_list(names);
+  std::vector<godel_surface_detection::detection::Cloud::Ptr> surface_clouds = surface_detection_.get_surface_clouds();
+  std::vector<godel_surface_detection::detection::Cloud::Ptr> selected_clouds;
+  int ind;
+  for(const auto& s : names)
+  {
+    std::string t = s;
+    t.erase(0, SURFACE_DESIGNATION.size());
+    ind = std::stoi(t) - 1;
+    selected_clouds.push_back(surface_clouds.at(ind));
+  }
+
 
   ROS_ASSERT(names.size() == meshes.size());
 
@@ -417,7 +471,7 @@ godel_surface_detection::TrajectoryLibrary SurfaceBlendingService::generateMotio
   godel_surface_detection::TrajectoryLibrary lib;
   for (std::size_t i = 0; i < meshes.size(); ++i)
   {
-    ProcessPathResult paths = generateProcessPath(names[i], meshes[i], blend_params, scan_params);
+    ProcessPathResult paths = generateProcessPath(names[i], meshes[i], selected_clouds[i], blend_params, scan_params);
     for (std::size_t j = 0; j < paths.paths.size(); ++j)
     {
       ProcessPlanResult plan = generateProcessPlan(paths.paths[j].first, paths.paths[j].second,
@@ -444,7 +498,7 @@ static inline bool isEdgePath(const std::string& name)
   const static std::string suffix("_edge");
   if (name.size() < suffix.size())
     return false;
-  return name.find(suffix, name.size() - suffix.length()) != std::string::npos;
+  return name.find(suffix) != std::string::npos;
 }
 
 ProcessPlanResult
diff --git a/godel_surface_detection/src/services/surface_blending_service.cpp b/godel_surface_detection/src/services/surface_blending_service.cpp
index 2bcf46fe..96e10e3f 100644
--- a/godel_surface_detection/src/services/surface_blending_service.cpp
+++ b/godel_surface_detection/src/services/surface_blending_service.cpp
@@ -1,5 +1,5 @@
 #include <godel_surface_detection/services/surface_blending_service.h>
-
+#include <godel_surface_detection/detection/surface_segmentation.h>
 #include <godel_msgs/TrajectoryExecution.h>
 
 // Process Execution
@@ -30,6 +30,8 @@ const static std::string BLEND_PROCESS_PLANNING_SERVICE = "blend_process_plannin
 const static std::string SCAN_PROCESS_PLANNING_SERVICE = "keyence_process_planning";
 
 const static std::string TOOL_PATH_PREVIEW_TOPIC = "tool_path_preview";
+const static std::string EDGE_VISUALIZATION_TOPIC = "edge_visualization";
+const static std::string BLEND_VISUALIZATION_TOPIC = "blend_visualization";
 const static std::string SELECTED_SURFACES_CHANGED_TOPIC = "selected_surfaces_changed";
 const static std::string ROBOT_SCAN_PATH_PREVIEW_TOPIC = "robot_scan_path_preview";
 const static std::string PUBLISH_REGION_POINT_CLOUD = "publish_region_point_cloud";
@@ -165,6 +167,12 @@ bool SurfaceBlendingService::init()
   tool_path_markers_pub_ =
       nh.advertise<visualization_msgs::MarkerArray>(TOOL_PATH_PREVIEW_TOPIC, 1, true);
 
+  blend_visualization_pub_ =
+      nh.advertise<geometry_msgs::PoseArray>(BLEND_VISUALIZATION_TOPIC, 1, true);
+
+  edge_visualization_pub_ =
+      nh.advertise<geometry_msgs::PoseArray>(EDGE_VISUALIZATION_TOPIC, 1, true);
+
   return true;
 }
 
@@ -299,7 +307,7 @@ bool SurfaceBlendingService::find_surfaces(visualization_msgs::MarkerArray& surf
       surface_server_.add_surface(meshes[i]);
     }
 
-    // copying to surface markers to output argument
+    // copying surface markers to output argument
     visualization_msgs::MarkerArray markers_msg = surface_detection_.get_surface_markers();
     surfaces.markers.insert(surfaces.markers.begin(), markers_msg.markers.begin(),
                             markers_msg.markers.end());
@@ -361,7 +369,13 @@ void SurfaceBlendingService::remove_previous_process_plan()
   markers.markers.insert(markers.markers.end(), scans.markers.begin(), scans.markers.end());
   markers.markers.insert(markers.markers.end(), edge.markers.begin(), edge.markers.end());
 
+  geometry_msgs::PoseArray empty_poses;
+  empty_poses.header.frame_id = "world_frame";
+  empty_poses.header.stamp = ros::Time::now();
+
   tool_path_markers_pub_.publish(markers);
+  edge_visualization_pub_.publish(empty_poses);
+  blend_visualization_pub_.publish(empty_poses);
 
   bds.markers.clear();
   paths.markers.clear();