diff --git a/src/surveillance_robot/res/Alien_Machine_Gun-Matt_Cutillo-2023875589.wav b/src/surveillance_robot/res/Alien_Machine_Gun-Matt_Cutillo-2023875589.wav deleted file mode 100644 index 36b5e23..0000000 Binary files a/src/surveillance_robot/res/Alien_Machine_Gun-Matt_Cutillo-2023875589.wav and /dev/null differ diff --git a/src/surveillance_robot/src/check_node.cpp b/src/surveillance_robot/src/check_node.cpp index cd30bf2..921637c 100644 --- a/src/surveillance_robot/src/check_node.cpp +++ b/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) { diff --git a/src/surveillance_robot/src/decision_node.cpp b/src/surveillance_robot/src/decision_node.cpp index 5089a70..9d9ebd7 100644 --- a/src/surveillance_robot/src/decision_node.cpp +++ b/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,16 +235,18 @@ 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; } } @@ -250,6 +254,9 @@ void update() { //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 /*////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/surveillance_robot/src/global_planner_node.cpp b/src/surveillance_robot/src/global_planner_node.cpp index c30f91b..79f69c4 100644 --- a/src/surveillance_robot/src/global_planner_node.cpp +++ b/src/surveillance_robot/src/global_planner_node.cpp @@ -15,6 +15,9 @@ #include #include #include +#include + + 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 (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 (rand()) / static_cast (RAND_MAX); + color.g = static_cast (rand()) / static_cast (RAND_MAX); + color.b = static_cast (rand()) / static_cast (RAND_MAX); + + + ROS_INFO_STREAM("Color :" << color); + + } }; diff --git a/src/surveillance_robot/src/person_detector_node.cpp b/src/surveillance_robot/src/person_detector_node.cpp index f0a774d..d9e7c5f 100644 --- a/src/surveillance_robot/src/person_detector_node.cpp +++ b/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"); diff --git a/src/surveillance_robot/src/translation_node.cpp b/src/surveillance_robot/src/translation_node.cpp index f69f960..cb435f4 100644 --- a/src/surveillance_robot/src/translation_node.cpp +++ b/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: