<commit_before>#include <yaml-cpp/yaml.h>
#include <ros/ros.h>
#include <vector>
#include <fstream>
#include <string>
#include <stdio.h>

// msgs
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h> 

#include <std_msgs/String.h>

// path
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>

#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Transform.h>
#include <tf/transform_datatypes.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <bwi_scavenger/VisionTaskAction.h>
#include <bwi_scavenger/TargetSearch.h>
#include <actionlib/client/terminal_state.h>

#include <math.h>

const float PI = atan(1) * 4; 

// global variables

geometry_msgs::PoseWithCovarianceStamped curr_pos; 
bwi_scavenger::VisionTaskGoal *goal; 

ros::NodeHandle * nh; 

actionlib::SimpleActionClient <bwi_scavenger::VisionTaskAction> * ac; 

// callback function that saves robot's current position
void callbackCurrPos(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
    
    // ROS_INFO("I heard something about my current position"); 
    curr_pos = *msg;
    
}


// compute the length of a path, nav_msgs::Path
// we used pixel as the distance unit, as we do not care the absolute values
// returns a float value
unsigned computePathLength(nav_msgs::Path * path) {
 
    float len = 0.0; 

    if (!path) {
        ROS_ERROR("Failed to compute path length as the path is empty"); 
        return 0.0; 
    }

    std::vector <geometry_msgs::PoseStamped> vec(path->poses);

    return (unsigned) vec.size(); 
}

// this function updates the belief distribution based on bayesian equation
void updateBelief(std::vector<float> *belief, bool detected, int next_goal_index) {
    
    float true_positive_rate, true_negative_rate; 

    ros::param::param <float> ("~true_positive_rate", true_positive_rate, 0.95); 
    ros::param::param <float> ("~true_negative_rate", true_negative_rate, 0.95); 
    
    std::vector <float> tmp_belief (*belief); 

    if (detected)
        tmp_belief[next_goal_index] = tmp_belief[next_goal_index] * true_positive_rate / 
            ((tmp_belief[next_goal_index] * true_positive_rate) + 
             ((1.0 - tmp_belief[next_goal_index]) * (1.0 - true_negative_rate))); 
    else
        tmp_belief[next_goal_index] = tmp_belief[next_goal_index] * (1.0 - true_positive_rate) / 
            ((tmp_belief[next_goal_index] * (1.0 - true_positive_rate)) + 
             ((1.0 - tmp_belief[next_goal_index]) * true_negative_rate)); 

    float normalizer = 0.0; 

    for (int i = 0; i < tmp_belief.size(); i++)
        normalizer += tmp_belief[i]; 

    for (int i = 0; i < tmp_belief.size(); i++)
        (*belief)[i]  = tmp_belief[i] / normalizer; 
        
}


// this function calls the robot platform and visual sensors to sense a specific
// scene. It return a boolean value identifies if the target is detected. 
bool observe() {

    ROS_INFO("%s: checking vision service availability", 
        ros::this_node::getName().c_str()); 
    // ac->waitForServer(); // will wait for infinite time
    ROS_INFO("passed"); 

    // publish to /cmd_vel to stop the robot first
    ros::Publisher pub = nh->advertise <geometry_msgs::Twist> ("/cmd_vel", 100); 
    geometry_msgs::Twist vel; 
    vel.linear.x = 0;
    vel.linear.y = 0;
    vel.angular.z = 0;
    pub.publish(vel); 

    ROS_INFO("Look to the left..."); // look to the left
    vel.angular.z = 0.1;

    for (int i=0; i < 100; i++) {
        ros::spinOnce();
        pub.publish(vel); 
        ros::Duration(0.1).sleep();
    }

    ROS_INFO("Look to the right..."); // look to the right
    vel.angular.z = -0.1;

    for (int i=0; i < 200; i++) {
        ros::spinOnce();
        pub.publish(vel); 
        ros::Duration(0.1).sleep();
    }

    vel.angular.z = 0;
    for (int i=0; i < 10; i++) {
        ros::spinOnce();
        pub.publish(vel); 
        ros::Duration(0.1).sleep();
    }

    // here we assume the robot can never detects the target, so it's always
    // returning false; 
    bool ret = ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    ROS_INFO("%s: observed? %d", ros::this_node::getName().c_str(), ret);
    return ret; 
}

bool  service_callback(bwi_scavenger::TargetSearch::Request &req, 
    bwi_scavenger::TargetSearch::Response &res) {

    ROS_INFO("%s: target search task received", ros::this_node::getName().c_str()); 

    goal->type = req.type;
    goal->path_to_template = req.path_to_template; 
    goal->color = req.color;

    ROS_INFO("%s: waiting for action service to start.",
        ros::this_node::getName().c_str()); 

    // ac->waitForServer(); // will wait for infinite time

    ROS_INFO("%s: action server started, sending goal.",
        ros::this_node::getName().c_str()); 

    ac->sendGoal( *goal ); 
    
    std::string yaml_file_positions; 

    if (ros::param::get("~positions", yaml_file_positions))
        ROS_INFO("\nFile: %s", yaml_file_positions.c_str()); 

    // std::ifstream fin(yaml_file_positions.c_str());
    // YAML::Parser parser(fin);
    // YAML::Node positions;
    // parser.GetNextDocument(positions); 
    YAML::Node positions = YAML::LoadFile(yaml_file_positions); 

    // belief distribution over all possible target positions
    std::vector <float> belief(positions.size(), 1.0/positions.size()); 

    // a vector that saves the distances to all possible positions
    std::vector <unsigned> distances(positions.size(), 0.0); 

    // fitness function: maximization to decide where to see
    std::vector <float> fitness(positions.size(), 0.0); 

    // service for computing a path from current position to a goal
    ros::ServiceClient client_compute_path = 
        nh->serviceClient <nav_msgs::GetPlan> ("move_base/NavfnROS/make_plan"); 
    nav_msgs::GetPlan srv; 

    // service for driving robot platform
    ros::Publisher pub_move_robot = nh->advertise <geometry_msgs::PoseStamped> 
        ("/move_base_interruptable_simple/goal", 100); 
    geometry_msgs::PoseStamped msg_goal; 

    // subscribe to the topic that reports the current position
    ros::Subscriber sub = nh->subscribe("amcl_pose", 100, callbackCurrPos); 
    ros::Duration(2.0).sleep(); 

    // get the parameter of tolerance to goal
    float tolerance; 
    if (!ros::param::get("~tolerance", tolerance)) 
        ROS_ERROR("tolerance value not set"); 

    float analyzing_cost;
    ros::param::param <float> ("~analyzing_scene_cost", analyzing_cost, 5.0); 

    bool detected = false;

    ros::Rate loop_rate(10); 

    std::stringstream ss;

    while (ros::ok()) {

        // to get the current position using the callback function
        ros::spinOnce(); 
        ROS_INFO("Compute which scene to analyze next"); 

        for (unsigned i = 0; i < positions.size(); i++) {

            // to determine the starting point
            srv.request.start.header.frame_id = "level_mux/map"; 
            srv.request.start.pose = curr_pos.pose.pose; 

            // to determine the goal point
            srv.request.goal.header.frame_id = "level_mux/map"; 
            // positions[i][0] >> srv.request.goal.pose.position.x; 
            // positions[i][1] >> srv.request.goal.pose.position.y; 
            srv.request.goal.pose.position.x = positions[i][0].as<double>(); 
            srv.request.goal.pose.position.y = positions[i][1].as<double>(); 

            // call service to compute a path to a possible positon
            client_compute_path.waitForExistence();
            if (!client_compute_path.call(srv))
                ROS_ERROR("Calling service for path computing failed"); 

            nav_msgs::Path path = srv.response.plan; 

            // compute and save the distance of that path
            distances[i] = computePathLength( & path); 

        }

        // fitness function, weighted belief probabilities
        int next_goal_index; 
        float tmp_max = -1.0; 
        float resolution;
        ros::param::param <float> ("/move_base/local_costmap/resolution", 
            resolution, 0.05); 

        for (unsigned i = 0; i < positions.size(); i++) {
            
            // exit of this program
            if (belief[i] > 0.8) {
                res.path_to_image = ac->getResult()->path_to_image; 
                return true; 
            }

            fitness[i] = belief[i] / ( (float) distances[i] * resolution + analyzing_cost); 
            
            // finds the largest fitness value and save its index to
            // next_goal_index
            if (fitness[i] >= tmp_max) {
                next_goal_index = i; 
                tmp_max = fitness[i];     
            }
        }
<commit_msg>increased belief threshold for target search tasks<commit_after>#include <yaml-cpp/yaml.h>
#include <ros/ros.h>
#include <vector>
#include <fstream>
#include <string>
#include <stdio.h>

// msgs
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h> 

#include <std_msgs/String.h>

// path
#include <nav_msgs/Path.h>
#include <nav_msgs/GetPlan.h>

#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Transform.h>
#include <tf/transform_datatypes.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <bwi_scavenger/VisionTaskAction.h>
#include <bwi_scavenger/TargetSearch.h>
#include <actionlib/client/terminal_state.h>

#include <math.h>

const float PI = atan(1) * 4; 

// global variables

geometry_msgs::PoseWithCovarianceStamped curr_pos; 
bwi_scavenger::VisionTaskGoal *goal; 

ros::NodeHandle * nh; 

actionlib::SimpleActionClient <bwi_scavenger::VisionTaskAction> * ac; 

// callback function that saves robot's current position
void callbackCurrPos(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
    
    // ROS_INFO("I heard something about my current position"); 
    curr_pos = *msg;
    
}


// compute the length of a path, nav_msgs::Path
// we used pixel as the distance unit, as we do not care the absolute values
// returns a float value
unsigned computePathLength(nav_msgs::Path * path) {
 
    float len = 0.0; 

    if (!path) {
        ROS_ERROR("Failed to compute path length as the path is empty"); 
        return 0.0; 
    }

    std::vector <geometry_msgs::PoseStamped> vec(path->poses);

    return (unsigned) vec.size(); 
}

// this function updates the belief distribution based on bayesian equation
void updateBelief(std::vector<float> *belief, bool detected, int next_goal_index) {
    
    float true_positive_rate, true_negative_rate; 

    ros::param::param <float> ("~true_positive_rate", true_positive_rate, 0.95); 
    ros::param::param <float> ("~true_negative_rate", true_negative_rate, 0.95); 
    
    std::vector <float> tmp_belief (*belief); 

    if (detected)
        tmp_belief[next_goal_index] = tmp_belief[next_goal_index] * true_positive_rate / 
            ((tmp_belief[next_goal_index] * true_positive_rate) + 
             ((1.0 - tmp_belief[next_goal_index]) * (1.0 - true_negative_rate))); 
    else
        tmp_belief[next_goal_index] = tmp_belief[next_goal_index] * (1.0 - true_positive_rate) / 
            ((tmp_belief[next_goal_index] * (1.0 - true_positive_rate)) + 
             ((1.0 - tmp_belief[next_goal_index]) * true_negative_rate)); 

    float normalizer = 0.0; 

    for (int i = 0; i < tmp_belief.size(); i++)
        normalizer += tmp_belief[i]; 

    for (int i = 0; i < tmp_belief.size(); i++)
        (*belief)[i]  = tmp_belief[i] / normalizer; 
        
}


// this function calls the robot platform and visual sensors to sense a specific
// scene. It return a boolean value identifies if the target is detected. 
bool observe() {

    ROS_INFO("%s: checking vision service availability", 
        ros::this_node::getName().c_str()); 
    // ac->waitForServer(); // will wait for infinite time
    ROS_INFO("passed"); 

    // publish to /cmd_vel to stop the robot first
    ros::Publisher pub = nh->advertise <geometry_msgs::Twist> ("/cmd_vel", 100); 
    geometry_msgs::Twist vel; 
    vel.linear.x = 0;
    vel.linear.y = 0;
    vel.angular.z = 0;
    pub.publish(vel); 

    ROS_INFO("Look to the left..."); // look to the left
    vel.angular.z = 0.1;

    for (int i=0; i < 100; i++) {
        ros::spinOnce();
        pub.publish(vel); 
        ros::Duration(0.1).sleep();
    }

    ROS_INFO("Look to the right..."); // look to the right
    vel.angular.z = -0.1;

    for (int i=0; i < 200; i++) {
        ros::spinOnce();
        pub.publish(vel); 
        ros::Duration(0.1).sleep();
    }

    vel.angular.z = 0;
    for (int i=0; i < 10; i++) {
        ros::spinOnce();
        pub.publish(vel); 
        ros::Duration(0.1).sleep();
    }

    // here we assume the robot can never detects the target, so it's always
    // returning false; 
    bool ret = ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    ROS_INFO("%s: observed? %d", ros::this_node::getName().c_str(), ret);
    return ret; 
}

bool  service_callback(bwi_scavenger::TargetSearch::Request &req, 
    bwi_scavenger::TargetSearch::Response &res) {

    ROS_INFO("%s: target search task received", ros::this_node::getName().c_str()); 

    goal->type = req.type;
    goal->path_to_template = req.path_to_template; 
    goal->color = req.color;

    ROS_INFO("%s: waiting for action service to start.",
        ros::this_node::getName().c_str()); 

    // ac->waitForServer(); // will wait for infinite time

    ROS_INFO("%s: action server started, sending goal.",
        ros::this_node::getName().c_str()); 

    ac->sendGoal( *goal ); 
    
    std::string yaml_file_positions; 

    if (ros::param::get("~positions", yaml_file_positions))
        ROS_INFO("\nFile: %s", yaml_file_positions.c_str()); 

    // std::ifstream fin(yaml_file_positions.c_str());
    // YAML::Parser parser(fin);
    // YAML::Node positions;
    // parser.GetNextDocument(positions); 
    YAML::Node positions = YAML::LoadFile(yaml_file_positions); 

    // belief distribution over all possible target positions
    std::vector <float> belief(positions.size(), 1.0/positions.size()); 

    // a vector that saves the distances to all possible positions
    std::vector <unsigned> distances(positions.size(), 0.0); 

    // fitness function: maximization to decide where to see
    std::vector <float> fitness(positions.size(), 0.0); 

    // service for computing a path from current position to a goal
    ros::ServiceClient client_compute_path = 
        nh->serviceClient <nav_msgs::GetPlan> ("move_base/NavfnROS/make_plan"); 
    nav_msgs::GetPlan srv; 

    // service for driving robot platform
    ros::Publisher pub_move_robot = nh->advertise <geometry_msgs::PoseStamped> 
        ("/move_base_interruptable_simple/goal", 100); 
    geometry_msgs::PoseStamped msg_goal; 

    // subscribe to the topic that reports the current position
    ros::Subscriber sub = nh->subscribe("amcl_pose", 100, callbackCurrPos); 
    ros::Duration(2.0).sleep(); 

    // get the parameter of tolerance to goal
    float tolerance; 
    if (!ros::param::get("~tolerance", tolerance)) 
        ROS_ERROR("tolerance value not set"); 

    float analyzing_cost;
    ros::param::param <float> ("~analyzing_scene_cost", analyzing_cost, 5.0); 

    bool detected = false;

    ros::Rate loop_rate(10); 

    std::stringstream ss;

    while (ros::ok()) {

        // to get the current position using the callback function
        ros::spinOnce(); 
        ROS_INFO("Compute which scene to analyze next"); 

        for (unsigned i = 0; i < positions.size(); i++) {

            // to determine the starting point
            srv.request.start.header.frame_id = "level_mux/map"; 
            srv.request.start.pose = curr_pos.pose.pose; 

            // to determine the goal point
            srv.request.goal.header.frame_id = "level_mux/map"; 
            // positions[i][0] >> srv.request.goal.pose.position.x; 
            // positions[i][1] >> srv.request.goal.pose.position.y; 
            srv.request.goal.pose.position.x = positions[i][0].as<double>(); 
            srv.request.goal.pose.position.y = positions[i][1].as<double>(); 

            // call service to compute a path to a possible positon
            client_compute_path.waitForExistence();
            if (!client_compute_path.call(srv))
                ROS_ERROR("Calling service for path computing failed"); 

            nav_msgs::Path path = srv.response.plan; 

            // compute and save the distance of that path
            distances[i] = computePathLength( & path); 

        }

        // fitness function, weighted belief probabilities
        int next_goal_index; 
        float tmp_max = -1.0; 
        float resolution;
        ros::param::param <float> ("/move_base/local_costmap/resolution", 
            resolution, 0.05); 

        for (unsigned i = 0; i < positions.size(); i++) {
            
            // exit of this program
            if (belief[i] > 0.9) {
                res.path_to_image = ac->getResult()->path_to_image; 
                return true; 
            }

            fitness[i] = belief[i] / ( (float) distances[i] * resolution + analyzing_cost); 
            
            // finds the largest fitness value and save its index to
            // next_goal_index
            if (fitness[i] >= tmp_max) {
                next_goal_index = i; 
                tmp_max = fitness[i];     
            }
        }

{'fraction_non_alphanumeric': 0.13266640140965158, 'fraction_numerical': 0.00989029727732621, 'mean_word_length': 2.7942635324563296, 'pattern_counts': {'":': 0, '<': 91, '<?xml version=': 0, '>': 117, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4650759', 'n_tokens_mistral': 6011, 'n_tokens_neox': 5415, 'n_words': 1326}