diff --git a/crosbot_explore/CMakeLists.txt b/crosbot_explore/CMakeLists.txt
index 0821a9477910e135e479fa8cfbbef2e5ccaab09f..a41865dab054a43e3e1c9476538c6fad7463fc82 100644
--- a/crosbot_explore/CMakeLists.txt
+++ b/crosbot_explore/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(crosbot_explore)
 
+add_compile_options(-ggdb)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -16,6 +18,8 @@ find_package(catkin REQUIRED COMPONENTS
     crosbot
     crosbot_map
     crosbot_msgs
+    pcl_ros
+
 )
 
 ## System dependencies are found with CMake's conventions
diff --git a/crosbot_explore/include/crosbot_explore/explorerROS.hpp b/crosbot_explore/include/crosbot_explore/explorerROS.hpp
index e92d14ae62816799dd4ed2193831fe60ec3dcf24..41ce6df4e2d547c05695a1928668d46c0c9c6817 100644
--- a/crosbot_explore/include/crosbot_explore/explorerROS.hpp
+++ b/crosbot_explore/include/crosbot_explore/explorerROS.hpp
@@ -24,6 +24,16 @@
 #include <ros/subscriber.h>
 #include <tf/transform_listener.h>
 
+#include <pcl_ros/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/common/pca.h>
+#include <pcl/common/transforms.h>
+
 #include <string>
 
 namespace crosbot {
@@ -62,6 +72,10 @@ protected:
     VoronoiGridPtr latestVoronoi;
     Pose latestPose;
 
+    sensor_msgs::PointCloud2::ConstPtr lastScan;
+    ros::Publisher pclPub;
+    ros::Subscriber laserSub;
+
 public:
     ExplorerROSNode();
     virtual ~ExplorerROSNode();
@@ -118,6 +132,9 @@ protected:
      */
     void publishReconfig(VoronoiGrid::Constraints& constraints);
 
+    void callbackLaserScan(const sensor_msgs::PointCloud2::ConstPtr& scan);
+    bool objectTooClose();
+
 private:
     // Allow local voronoi constraints for this explorer (this allows the constraints to be reset between different modes)
     VoronoiGrid::Constraints localVoronoiConstraints;
diff --git a/crosbot_explore/package.xml b/crosbot_explore/package.xml
index 0f3b8485b8060bfa96d79f25ffca2b4edf2f274b..ccbc8a1c188678920a1155c929003de15616bd96 100644
--- a/crosbot_explore/package.xml
+++ b/crosbot_explore/package.xml
@@ -39,7 +39,8 @@
     <depend>dynamic_reconfigure</depend>
     <depend>geometry_msgs</depend>
     <depend>nav_msgs</depend>
-    
+    <depend>pcl_ros</depend>
+      
     <depend>message_generation</depend>
     <depend>message_runtime</depend>
     
@@ -47,4 +48,4 @@
     <doc_depend>rosdoc_lite</doc_depend>
     <doc_depend>doxygen</doc_depend>
 
-</package>
\ No newline at end of file
+</package>
diff --git a/crosbot_explore/src/basicClient/basicClient.cpp b/crosbot_explore/src/basicClient/basicClient.cpp
index 6d28bc423bf90bbb37c061360da080f4e80f2266..b1f4c7e714f58cd6933bc395ec9d00e37c9de56d 100644
--- a/crosbot_explore/src/basicClient/basicClient.cpp
+++ b/crosbot_explore/src/basicClient/basicClient.cpp
@@ -16,7 +16,7 @@
 #include <std_msgs/String.h>
 
 #define CONTROL_COMMAND_RESET_MAP       "reset_map"
-#define DEFAULT_SERVER_WAIT             0.5
+#define DEFAULT_SERVER_WAIT             1.5
 #define LOG_START                       "BasicExplorerClient ::"
 
 
diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp
index d8d9b816270faf60b3dd21fcc818f1fa9d4fb22b..a4e41771ef02113c5a8d72322994904f522c4e4b 100644
--- a/crosbot_explore/src/explorer.cpp
+++ b/crosbot_explore/src/explorer.cpp
@@ -721,7 +721,7 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot)
 	    // Note: Selected waypoint may be zero here, and waypoints empty
 		// If we are in an intermediate waypoint, we used a relaxed distance
 	    int selectedWaypoint = searchParams.currentWaypoint;
-	    while ( 
+	    while ( searchParams.waypoints.size() > 0 &&
 			// All waypoints except the last one
 			(selectedWaypoint < (searchParams.waypoints.size()-1) && searchParams.waypoints[selectedWaypoint].position.distanceTo(robot.position) < searchParams.waypointDistance) ||
 			// Last waypoint
diff --git a/crosbot_explore/src/explorerROS.cpp b/crosbot_explore/src/explorerROS.cpp
index e507316ba0c5d3c490158de9f1fda081335f55b3..a9f8b54cb5edd1e98a9501e1b8dd3ff261eb036a 100644
--- a/crosbot_explore/src/explorerROS.cpp
+++ b/crosbot_explore/src/explorerROS.cpp
@@ -15,9 +15,9 @@
 #include <std_msgs/String.h>
 
 
-#define DEFAULT_BASEFRAME			    "/base_link"
+#define DEFAULT_BASEFRAME			    "/base_footprint"
 #define DEFAULT_DRIVE_OPERATING_RATE    100
-#define DEFAULT_LOADPOSE_WAIT4TRANSFORM 0.5
+#define DEFAULT_LOADPOSE_WAIT4TRANSFORM 10.0
 #define LOG_START                       "CrosbotExploreROS ::"
 
 namespace crosbot {
@@ -34,6 +34,11 @@ ExplorerROSNode::ExplorerROSNode() :
 ExplorerROSNode::~ExplorerROSNode() {
 };
 
+
+void ExplorerROSNode::callbackLaserScan(const sensor_msgs::PointCloud2::ConstPtr& scan) {
+    this->lastScan = scan;
+}
+
 void ExplorerROSNode::callbackOccGrid(const nav_msgs::OccupancyGridConstPtr& latestMap) {
     Lock lock(lock_ros, true);
     //ROS_INFO("%s callbackOccGrid", LOG_START);
@@ -252,6 +257,9 @@ void ExplorerROSNode::configure() {
     historySub = nh.subscribe(historySub_name, 1, &ExplorerROSNode::callbackHistory, this);
     voronoiImagePub = nh.advertise<sensor_msgs::Image>(imagePub_name, 1);
     velPub = nh.advertise<geometry_msgs::Twist>(velPub_name, 1);
+    this->lastScan = nullptr;
+    laserSub = nh.subscribe("/urg_cloud" , 1, &ExplorerROSNode::callbackLaserScan, this);
+    pclPub = nh.advertise<sensor_msgs::PointCloud2>("pclout", 1);
 
     //Set pidControl to smooth velocity
     pidControl.setP(driveParams.velP);
@@ -502,6 +510,10 @@ void ExplorerROSNode::driveTo(const Pose& relativePosition) {
         twist.angular.z = 0;
     }
 
+    if(objectTooClose()){
+        twist.linear.x = 0;
+    }
+
     velPub.publish(twist);
 }
 
@@ -598,5 +610,57 @@ void ExplorerROSNode::statusChanged(crosbot_explore::ExplorerStatus::Status stat
     explorer_feedback_pub.publish(feedback);
 }
 
+bool ExplorerROSNode::objectTooClose(){
+    if(this->lastScan == nullptr){
+        return false;
+    }
+
+    sensor_msgs::PointCloud2::ConstPtr laser = this->lastScan;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);;
+    pcl::fromROSMsg (*laser, *pclCloud);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRotated = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    if(laser->header.frame_id.compare("/base_range_sensor_link") == 0){
+        cloudRotated = pclCloud;
+    } else {
+        pcl_ros::transformPointCloud ("/base_range_sensor_link", *pclCloud, *cloudRotated, tfListener);
+        
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);;
+    pcl::PassThrough<pcl::PointXYZ> pass;
+    pass.setInputCloud (cloudRotated);
+    pass.setFilterFieldName ("y");
+    pass.setFilterLimits (-0.3, 0.3);
+    pass.filter (*filteredCloud);
+
+
+    tf::Vector3 origin;
+    pcl::PointXYZ centre(0.2, 0.0, 0.2);
+    int count = 0;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr collideCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);;
+    for(auto it = filteredCloud->begin(); it != filteredCloud->end(); it++){ 
+        float distance = std::sqrt((it->x)*(it->x) + (it->y)*(it->y));
+        if(distance < 0.35){
+            collideCloud->points.push_back(*it);
+            count ++;
+        }
+    }
+    if(count > 10){
+        ROS_ERROR("Found collision");
+    }
+
+    sensor_msgs::PointCloud2 out;
+    pcl::toROSMsg(*collideCloud, out);
+    out.header.frame_id = "/base_range_sensor_link";
+    pclPub.publish(out);
+
+    
+
+
+    return (count < 10)?false:true;
+}
+
 } // namespace crosbot
 
diff --git a/crosbot_map/src/nodes/localmapFromCloud.cpp b/crosbot_map/src/nodes/localmapFromCloud.cpp
index 0cb8843eac63a183e791da22b005d34f5efb43ff..5d32aa6db8014846ac763852c98836bf6b950de6 100644
--- a/crosbot_map/src/nodes/localmapFromCloud.cpp
+++ b/crosbot_map/src/nodes/localmapFromCloud.cpp
@@ -29,7 +29,7 @@
 
 using namespace crosbot;
 
-#define DEFAULT_MAPFRAME			"/odom"
+#define DEFAULT_MAPFRAME			"/map"
 #define DEFAULT_BASEFRAME			"/base_link"
 
 #define DEFAULT_MAPWIDTH			10.0