Trial with the static laser

laser_scanner/CMakeLists.txt

@@ -143,6 +143,9 @@ add_executable(scanners src/lasers_fusion.cpp)
 target_link_libraries(scanners ${catkin_LIBRARIES})
 add_dependencies(scanners ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_executable(static src/static_laser.cpp)
+target_link_libraries(static ${catkin_LIBRARIES})
+add_dependencies(static ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the

laser_scanner/src/static_laser.cpp

@@ -0,0 +1,65 @@
+
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include "sensor_msgs/LaserScan.h"
+#include <vector>
+#include <math.h> 
+
+using namespace std;
+int Samples = 720;
+#define PI 3.1415926
+// The Sensor angle range is from -2/3 PI to 2/3PI, and the angle increment is  0.00672220578417 Deg
+std::vector<float> old_range = std::vector<float>(Samples, 0);
+std::vector<float> range = std::vector<float>(Samples, 0); 
+std::vector<float> diff = std::vector<float>(Samples,0);
+
+void scanValues(const sensor_msgs::LaserScan::ConstPtr& scan)
+{   
+    /*The Laser has 720 Samples */
+
+    range = scan->ranges;
+    double angle,x,y;
+    float safe_d = 2;
+    float warn_d = 1.5;
+    float distance_now;
+    //cout<<range[100]<<endl;
+    for(int i = 1; i<=Samples;i++)
+    {   
+        diff[i] = range[i] - old_range[i];
+
+        if ( abs(diff[i]) > 0.1 && abs(diff[i]) < 100 ) {
+
+        distance_now = diff[i];
+        //distance_now = sqrt(x*x+y*y); 
+
+        if(distance_now>safe_d)  {
+                ROS_INFO("Safety!");
+
+            }
+           else  if(distance_now>warn_d&&distance_now<safe_d) {
+
+                ROS_INFO("Critical!");                 
+            }
+            else if (distance_now<(warn_d)) {   
+                ROS_INFO("Dangerous!");
+            }
+
+        }
+
+    }
+   old_range = range;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "static");
+  ros::NodeHandle n;
+  ros::Subscriber sub;
+  //sub = n.subscribe("/mybot/laser/scan", 50, scanValues);
+  sub = n.subscribe("/sick_safetyscanners/scan", 50, scanValues);
+
+
+  ros::spin();
+  return 0;
+}
+