<commit_before>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/PoseArray.h>
#include <spatiotemporalexploration/PlanAction.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include "spatiotemporalexploration/Reachable.h"
#include "spatiotemporalexploration/Entropy.h"
#include "spatiotemporalexploration/AddView.h"
#include "spatiotemporalexploration/Visualize.h"
#include "spatiotemporalexploration/SaveLoad.h"
#include "nav_msgs/GetPlan.h"
#include "order.h"

#define MAX_ENTROPY 100000

using namespace std;

double MIN_X,MIN_Y,MIN_Z,RESOLUTION;
int DIM_X,DIM_Y,DIM_Z;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

ros::ServiceClient *save_client_ptr, *plan_client_ptr;

//typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
typedef actionlib::SimpleActionServer<spatiotemporalexploration::PlanAction> Server;

typedef struct
{
    int x;
    int y;
    double value;
}
maxima;

int numCellsX, numCellsY;

float *reachability_grid_ptr;

//Parameteres
double sensor_range, entropies_step;
int radius;

//MoveBaseClient *ac_ptr;

tf::TransformListener *tf_listener_ptr;

ros::ServiceClient *entropy_client_ptr;

ros::Publisher *points_pub_ptr, *max_pub_ptr, *reach_pub_ptr;

bool visualizedGrid(spatiotemporalexploration::Visualize::Request  &req, spatiotemporalexploration::Visualize::Response &res)
{

    //Markers Initialization
    visualization_msgs::MarkerArray points_markers;

    visualization_msgs::Marker test_point;
    test_point.header.frame_id = "/map";
    test_point.header.stamp = ros::Time::now();
    test_point.ns = "my_namespace";
    test_point.action = visualization_msgs::Marker::ADD;
    test_point.type = visualization_msgs::Marker::CUBE;
    test_point.color.a = 0.8;
    test_point.pose.position.z = 0.1;
    test_point.pose.orientation.w = 1.0;
    test_point.scale.x = entropies_step;
    test_point.scale.y = entropies_step;

    ROS_INFO("publishing reachability markers...");

    int grid_ind = 0;
    for(int j = 0; j < numCellsY; j++)
    {
        for(int i = 0; i < numCellsX; i++)
        {
            test_point.pose.position.x = MIN_X + entropies_step*(i+0.5);
            test_point.pose.position.y = MIN_Y + entropies_step*(j+0.5);
            test_point.id = grid_ind;
            test_point.color.r = 1.0 - reachability_grid_ptr[grid_ind];
            test_point.color.g = reachability_grid_ptr[grid_ind];
            test_point.color.b = 0.0;
            test_point.scale.z = 0.01 + reachability_grid_ptr[grid_ind];
            test_point.pose.position.z = test_point.scale.z/2;
            points_markers.markers.push_back(test_point);
            grid_ind++;

        }
    }

    reach_pub_ptr->publish(points_markers);
}

bool loadGrid(spatiotemporalexploration::SaveLoad::Request  &req, spatiotemporalexploration::SaveLoad::Response &res)
{

    FILE* file = fopen(req.filename.c_str(),"r");
    if (file!=NULL)
    {
        ROS_INFO("loading reachability grid...");
        int dummyX,dummyY;
        int grid_ind = 0;
        for(int j = 0; j < numCellsY; j++)
        {
            for(int i = 0; i < numCellsX; i++)
            {
                int fuck=fscanf(file, "%03i %03i %f\n",&dummyX,&dummyY,&reachability_grid_ptr[grid_ind]);
                grid_ind++;
            }
        }

        fclose(file);
        res.result = true;
    }
    else
    {<commit_msg>reachability grid visualization<commit_after>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/PoseArray.h>
#include <spatiotemporalexploration/PlanAction.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include "spatiotemporalexploration/Reachable.h"
#include "spatiotemporalexploration/Entropy.h"
#include "spatiotemporalexploration/AddView.h"
#include "spatiotemporalexploration/Visualize.h"
#include "spatiotemporalexploration/SaveLoad.h"
#include "nav_msgs/GetPlan.h"
#include "order.h"

#define MAX_ENTROPY 100000

using namespace std;

double MIN_X,MIN_Y,MIN_Z,RESOLUTION;
int DIM_X,DIM_Y,DIM_Z;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

ros::ServiceClient *save_client_ptr, *plan_client_ptr;

//typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
typedef actionlib::SimpleActionServer<spatiotemporalexploration::PlanAction> Server;

typedef struct
{
    int x;
    int y;
    double value;
}
maxima;

int numCellsX, numCellsY;

float *reachability_grid_ptr;

//Parameteres
double sensor_range, entropies_step;
int radius;

//MoveBaseClient *ac_ptr;

tf::TransformListener *tf_listener_ptr;

ros::ServiceClient *entropy_client_ptr;

ros::Publisher *points_pub_ptr, *max_pub_ptr, *reach_pub_ptr;

bool visualizedGrid(spatiotemporalexploration::Visualize::Request  &req, spatiotemporalexploration::Visualize::Response &res)
{

    //Markers Initialization
    visualization_msgs::MarkerArray points_markers;

    visualization_msgs::Marker test_point;
    test_point.header.frame_id = "/map";
    test_point.header.stamp = ros::Time::now();
    test_point.ns = "my_namespace";
    test_point.action = visualization_msgs::Marker::ADD;
    test_point.type = visualization_msgs::Marker::CUBE;
    test_point.color.a = 0.8;
    test_point.pose.position.z = 0.1;
    test_point.pose.orientation.w = 1.0;
    test_point.scale.x = entropies_step;
    test_point.scale.y = entropies_step;

    ROS_INFO("publishing reachability markers...");

    int grid_ind = 0;
    for(int j = 0; j < numCellsY; j++)
    {
        for(int i = 0; i < numCellsX; i++)
        {
            test_point.pose.position.x = MIN_X + entropies_step*(i+0.5);
            test_point.pose.position.y = MIN_Y + entropies_step*(j+0.5);
            test_point.id = grid_ind;
            test_point.color.r = 1.0 - reachability_grid_ptr[grid_ind];
            test_point.color.g = reachability_grid_ptr[grid_ind];
            test_point.color.b = 0.0;
            test_point.scale.z = 0.01 + reachability_grid_ptr[grid_ind];
            test_point.pose.position.z = test_point.scale.z/2;
            points_markers.markers.push_back(test_point);
            grid_ind++;

        }
    }

    reach_pub_ptr->publish(points_markers);

    return true;
}

bool loadGrid(spatiotemporalexploration::SaveLoad::Request  &req, spatiotemporalexploration::SaveLoad::Response &res)
{

    FILE* file = fopen(req.filename.c_str(),"r");
    if (file!=NULL)
    {
        ROS_INFO("loading reachability grid...");
        int dummyX,dummyY;
        int grid_ind = 0;
        for(int j = 0; j < numCellsY; j++)
        {
            for(int i = 0; i < numCellsX; i++)
            {
                int fuck=fscanf(file, "%03i %03i %f\n",&dummyX,&dummyY,&reachability_grid_ptr[grid_ind]);
                grid_ind++;
            }
        }

        fclose(file);
        res.result = true;
    }
    else
    {
{'fraction_non_alphanumeric': 0.12022533663094256, 'fraction_numerical': 0.00934322616103325, 'mean_word_length': 3.7327698309492847, 'pattern_counts': {'":': 0, '<': 35, '<?xml version=': 0, '>': 29, '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': '7560240', 'n_tokens_mistral': 2651, 'n_tokens_neox': 2534, 'n_words': 396}