final touches

src/surveillance_robot/src/check_node.cpp

@@ -3,7 +3,7 @@
 #include "geometry_msgs/PoseWithCovarianceStamped.h"
 #include "std_msgs/Bool.h"
 
-#define RECALCULATION_TRESHOLD 0.5
+#define RECALCULATION_THRESHOLD 0.6
 
 class check_node {
     private:
@@ -56,7 +56,7 @@ class check_node {
         }
 
         bool recalculationNeeded() {
-            return distancePoints(next_point, current_position_estim) > RECALCULATION_TRESHOLD;
+            return distancePoints(next_point, current_position_estim) > RECALCULATION_THRESHOLD;
         }
 
         void getNextPoint(const geometry_msgs::Point incoming_next_point) {

src/surveillance_robot/src/decision_node.cpp

@@ -98,7 +98,7 @@ decision() {
     new_check = false;
     new_global_goal = false;
 
-    debug = true;
+    debug = false;
 
     //INFINITE LOOP TO COLLECT LASER DATA AND PROCESS THEM
     ros::Rate r(100);// this node will work at 10hz
@@ -117,6 +117,7 @@ void update() {
 
     if ( display_state ) {
         display_state = false;
+
         ROS_INFO("============state: %i============", state);
     }
 
@@ -145,9 +146,10 @@ void update() {
 
       if( local_goal.x ==0 && local_goal.y == 0 && local_goal.z == 0 ){
         state = 0;
-        ROS_INFO("Reached the final position");
+        ROS_INFO("Reached the final position. Waiting for a new global goal");
       }
       else{
+        wait();
           pub_local_planner.publish(local_goal);
           state = 2;
       }
@@ -233,23 +235,28 @@ void update() {
       if(check_reply == false){
         ROS_INFO("(decision_node) /Our position is correct");
         wait_user_input();
-
-
-        local_goal = get_next_point();
-        pub_local_planner.publish(local_goal);
-
-        if( local_goal.x ==0 && local_goal.y == 0 && local_goal.z == 0 ){
+        if (path.size() == 0){
           state = 0;
-          ROS_INFO("Reached the final position");
+          ROS_INFO("Reached the final position.");
+          system("aplay -t wav -d 5 -f cd ~/catkin_ws/src/surveillance_robot/res/KoolTheGang-CelebrationRadio_2.wav");
+          ROS_INFO("Waiting for a new Global goal.");
         }
+
+
         else{
+          local_goal = get_next_point();
+          wait();
+          pub_local_planner.publish(local_goal);
           state = 2;
         }
       }
 
       //our position is not correct
       else{
         ROS_INFO("(decision_node) /Our position is not correct");
+        while(path.size() > 0){
+          path.pop();
+        }
         wait_user_input();
         std_msgs::Float32 msg_rotation_to_do;
         pub_global_planner.publish(global_goal);
@@ -288,6 +295,10 @@ void wait_user_input(){
   }
 }
 
+void wait(){
+  ros::Rate wait_rate(0.3);
+  wait_rate.sleep();
+}
 
 //CALLBACKS
 /*//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

src/surveillance_robot/src/global_planner_node.cpp

@@ -15,6 +15,9 @@
 #include <tf/transform_datatypes.h>
 #include <vector>
 #include <visualization_msgs/Marker.h>
+#include <cstdlib>
+
+
 class global_planner {
 private:
   ros::NodeHandle n;
@@ -46,8 +49,11 @@ class global_planner {
 
   visualization_msgs::Marker marker;
 
+  std_msgs::ColorRGBA color;
+
 public:
   global_planner() {
+    srand (static_cast <unsigned> (time(0)));
 
     // Communication with decision node
     sub_goal = n.subscribe("global_planner/global_goal", 1,
@@ -76,7 +82,7 @@ class global_planner {
     marker.scale.x = 0.5;
     marker.scale.y = 0.5;
     marker.scale.z = 0.5;
-
+    //
     // marker.color.a = 0.5;
     // marker.color.r = 0.0;
     // marker.color.g = 0.0;
@@ -103,18 +109,8 @@ class global_planner {
       marker.colors.push_back(c);
     }
 
-<<<<<<< HEAD
-    // ros::Rate rate(100);
-    // while(pub_path_rviz.getNumSubscribers() == 0){
-    //   rate.sleep();
-    // }
-
-    // pub_path_rviz.publish(marker);
-    // ROS_INFO("Published to rviz");
-=======
     pub_path_rviz.publish(marker);
     ROS_INFO("Published to rviz");
->>>>>>> a4334acb48387b29193b0ed2016ce027ee4eebd5
 
     graph = mparser.getAdjacencyMatrix();
     estimate_graph_position();
@@ -171,10 +167,10 @@ class global_planner {
         //fill the marker message
         geometry_msgs::Point p;
         std_msgs::ColorRGBA c;
-        c.a = 1.0;
-        c.g = 1.0;
-        c.r = 0.0;
-        c.b = 0.0;
+        c.a = color.a;
+        c.g = color.g;
+        c.r = color.r;
+        c.b = color.b;
         p.x = pose.position.x;
         p.y = pose.position.y;
         marker.points.push_back(p);
@@ -347,15 +343,6 @@ class global_planner {
     new_initial_position = true;
   }
 
-  void getPointGoal(const geometry_msgs::Point::ConstPtr &msg) {
-    ROS_INFO_STREAM("Received new global goal: " << msg);
-    goal_to_reach.position.x = msg->x;
-    goal_to_reach.position.y = msg->y;
-    goal_to_reach.position.z = 0;
-    ROS_INFO_STREAM("Saved as goal_to_reach :" << goal_to_reach);
-    new_goal_to_reach = true;
-  }
-
   void final_goal_to_reachCallback(const geometry_msgs::Point::ConstPtr &msg) {
     ROS_INFO_STREAM("Received global goal");
     goal_to_reach.position.x = msg->x;
@@ -365,6 +352,15 @@ class global_planner {
     finalPathPoint.erase(finalPathPoint.begin(), finalPathPoint.end());
 
 
+    color.a = 1;
+    color.r = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
+    color.g = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
+    color.b = static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
+
+
+    ROS_INFO_STREAM("Color :" << color);
+
+
   }
 };
 

src/surveillance_robot/src/person_detector_node.cpp

@@ -14,8 +14,8 @@
 #define cluster_threshold 0.2 //threshold for clustering
 
 //used for detection of motion
-#define detection_threshold 0.05 //threshold for motion detection
-#define dynamic_threshold 5 //to decide if a cluster is static or dynamic
+#define detection_threshold 0.4 //threshold for motion detection
+#define dynamic_threshold 95 //to decide if a cluster is static or dynamic
 
 //used for detection of moving legs
 #define leg_size_min 0.05
@@ -141,8 +141,11 @@ void update() {
             populateMarkerTopic();
 
             //to publish the goal_to_reach
-            if ( nb_moving_persons_detected )
-                pub_alarm.publish(true);
+            if ( nb_moving_persons_detected ){
+                ROS_INFO("Person detected, shooting!!!!");
+                system("aplay ~/catkin_ws/src/surveillance_robot/res/gun.wav");
+                nb_moving_persons_detected = 0;
+              }
         }
         else
             ROS_INFO("robot is moving");

src/surveillance_robot/src/translation_node.cpp

@@ -12,9 +12,9 @@ using namespace std;
 
 #define safety_distance 0.2
 #define translation_error 0.04
-#define kp 0.4
+#define kp 0.3
 #define ki 0.0001
-#define kd 0.2
+#define kd 0.1
 
 class translation {
 private: