<commit_before>#include <stdlib.h>
#include "ros/ros.h"
#include <mongodb_store/message_store.h>
#include <tf/tf.h>
#include <dynamic_reconfigure/server.h>
#include <infremen/infremenConfig.h>
#include <strands_navigation_msgs/NavStatistics.h>
#include <strands_navigation_msgs/TopologicalMap.h>
#include <strands_navigation_msgs/TopologicalNode.h>
#include <strands_executive_msgs/AddTask.h>
#include <strands_executive_msgs/CancelTask.h>
#include <strands_executive_msgs/SetExecutionStatus.h>
#include <strands_executive_msgs/CreateTask.h>
#include <mongodb_store_msgs/StringPair.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/PoseStamped.h>
#include <strands_navigation_msgs/GetTaggedNodes.h>
#include <std_srvs/Empty.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <scitos_msgs/BatteryState.h>
#include <info_task/Clicks.h>
#include <infremen/InfremenResult.h>
#include <infremen/AtomicInteraction.h>
#include "CFrelementSet.h"
#include <time.h> 
#include "mongodb_store/message_store.h"
#include "geometry_msgs/Pose.h"
#include <boost/foreach.hpp>
#include <sstream>
#include <cassert>

using namespace mongodb_store;
using namespace std;

//FIXED parameters
int windowDuration = 300;
int rescheduleInterval = 86400;

//standard parameters
string collectionName;
string scheduleDirectory;

//runtine parameters
float explorationRatio = 0.5;
int8_t   minimalBatteryLevel = 50;
int8_t   batteryUndockLevel = 55;
int32_t   minimalBatteryLevelTime = 0;
int interactionTimeout = 30;
int maxTaskNumber = 5;
int taskDuration = 180;	
int taskPriority = 1;	
bool debug = true;
int taskStartDelay = 5;
int rescheduleCheckTime = 5;

//ROS communication
ros::NodeHandle *n;
MessageStoreProxy *messageStore;
ros::Subscriber robotPoseSub;
ros::Subscriber currentNodeSub;
ros::Subscriber interfaceSub; 
ros::Subscriber batterySub;
ros::Subscriber infoTaskSub;
ros::Subscriber guiSub; 
ros::ServiceClient nodeListClient;
ros::ServiceClient taskAdder;
ros::ServiceClient taskCreator;
ros::ServiceClient taskCancel;

const mongo::BSONObj EMPTY_BSON_OBJ;

//fremen component
CFrelementSet frelementSet;

//state variables
int lastTimeSlot = -1;
int currentTimeSlot = -1;
int numCurrentTasks = 0; 
string nodeName = "ChargingPoint";
string closestNode = "ChargingPoint";
int32_t lastInteractionTime = -1;
geometry_msgs::Pose lastPose;
bool forceCharging = false;
int timeOffset = 0;

//times and nodes of schedule
uint32_t timeSlots[10000];
int nodes[10000];
int taskIDs[10000];
int numNodes = 0;

uint32_t getMidnightTime(uint32_t givenTime)
{
	return ((givenTime+timeOffset)/rescheduleInterval)*rescheduleInterval-timeOffset;
}

//parameter reconfiguration
void reconfigureCallback(infremen::infremenConfig &config, uint32_t level) 
{
    ROS_INFO("Reconfigure Request: %lf %d %d %d %d %d", config.explorationRatio, config.minimalBatteryLevel, config.batteryUndockLevel, config.interactionTimeout, config.maxTaskNumber, config.taskDuration);
	explorationRatio = config.explorationRatio;
	minimalBatteryLevel = config.minimalBatteryLevel;
    batteryUndockLevel = config.batteryUndockLevel;
	interactionTimeout = config.interactionTimeout;
	maxTaskNumber = config.maxTaskNumber;
	taskDuration = config.taskDuration;
	taskPriority = config.taskPriority;
	debug = config.verbose;
	taskStartDelay = config.taskStartDelay;
	rescheduleCheckTime = config.rescheduleCheckTime;
}

//listen to battery and set forced charging if necessary
void batteryCallBack(const scitos_msgs::BatteryState &msg)
{
	/*TODO learn from experience about energy consumption, plan ahead*/
	ROS_DEBUG("Infremen: Battery level %i %i",msg.lifePercent,minimalBatteryLevel);
    if (!forceCharging){
        if (minimalBatteryLevel > msg.lifePercent) forceCharging = true;
    }
    else{
        if (batteryUndockLevel < msg.lifePercent) forceCharging = false;
    }
}	

/*get robot pose*/
void poseCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
	lastPose = *msg;
	//if (debug) ROS_INFO("Infremen: Robot at %lf %lf %lf.",lastPose.position.x,lastPose.position.y,lastPose.position.z);
}

/*detailed info on interation*/
void interacted(const std_msgs::Int32::ConstPtr& msg)
{
	char testTime[1000];
	ros::Time currentTime = ros::Time::now();
	lastInteractionTime = currentTime.sec;

	time_t timeInfo = currentTime.sec;<commit_msg>using the same indentation as original author<commit_after>#include <stdlib.h>
#include "ros/ros.h"
#include <mongodb_store/message_store.h>
#include <tf/tf.h>
#include <dynamic_reconfigure/server.h>
#include <infremen/infremenConfig.h>
#include <strands_navigation_msgs/NavStatistics.h>
#include <strands_navigation_msgs/TopologicalMap.h>
#include <strands_navigation_msgs/TopologicalNode.h>
#include <strands_executive_msgs/AddTask.h>
#include <strands_executive_msgs/CancelTask.h>
#include <strands_executive_msgs/SetExecutionStatus.h>
#include <strands_executive_msgs/CreateTask.h>
#include <mongodb_store_msgs/StringPair.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/PoseStamped.h>
#include <strands_navigation_msgs/GetTaggedNodes.h>
#include <std_srvs/Empty.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <scitos_msgs/BatteryState.h>
#include <info_task/Clicks.h>
#include <infremen/InfremenResult.h>
#include <infremen/AtomicInteraction.h>
#include "CFrelementSet.h"
#include <time.h> 
#include "mongodb_store/message_store.h"
#include "geometry_msgs/Pose.h"
#include <boost/foreach.hpp>
#include <sstream>
#include <cassert>

using namespace mongodb_store;
using namespace std;

//FIXED parameters
int windowDuration = 300;
int rescheduleInterval = 86400;

//standard parameters
string collectionName;
string scheduleDirectory;

//runtine parameters
float explorationRatio = 0.5;
int8_t   minimalBatteryLevel = 50;
int8_t   batteryUndockLevel = 55;
int32_t   minimalBatteryLevelTime = 0;
int interactionTimeout = 30;
int maxTaskNumber = 5;
int taskDuration = 180;	
int taskPriority = 1;	
bool debug = true;
int taskStartDelay = 5;
int rescheduleCheckTime = 5;

//ROS communication
ros::NodeHandle *n;
MessageStoreProxy *messageStore;
ros::Subscriber robotPoseSub;
ros::Subscriber currentNodeSub;
ros::Subscriber interfaceSub; 
ros::Subscriber batterySub;
ros::Subscriber infoTaskSub;
ros::Subscriber guiSub; 
ros::ServiceClient nodeListClient;
ros::ServiceClient taskAdder;
ros::ServiceClient taskCreator;
ros::ServiceClient taskCancel;

const mongo::BSONObj EMPTY_BSON_OBJ;

//fremen component
CFrelementSet frelementSet;

//state variables
int lastTimeSlot = -1;
int currentTimeSlot = -1;
int numCurrentTasks = 0; 
string nodeName = "ChargingPoint";
string closestNode = "ChargingPoint";
int32_t lastInteractionTime = -1;
geometry_msgs::Pose lastPose;
bool forceCharging = false;
int timeOffset = 0;

//times and nodes of schedule
uint32_t timeSlots[10000];
int nodes[10000];
int taskIDs[10000];
int numNodes = 0;

uint32_t getMidnightTime(uint32_t givenTime)
{
	return ((givenTime+timeOffset)/rescheduleInterval)*rescheduleInterval-timeOffset;
}

//parameter reconfiguration
void reconfigureCallback(infremen::infremenConfig &config, uint32_t level) 
{
	ROS_INFO("Reconfigure Request: %lf %d %d %d %d %d", config.explorationRatio, config.minimalBatteryLevel, config.batteryUndockLevel, config.interactionTimeout, config.maxTaskNumber, config.taskDuration);
	explorationRatio = config.explorationRatio;
	minimalBatteryLevel = config.minimalBatteryLevel;
	batteryUndockLevel = config.batteryUndockLevel;
	interactionTimeout = config.interactionTimeout;
	maxTaskNumber = config.maxTaskNumber;
	taskDuration = config.taskDuration;
	taskPriority = config.taskPriority;
	debug = config.verbose;
	taskStartDelay = config.taskStartDelay;
	rescheduleCheckTime = config.rescheduleCheckTime;
}

//listen to battery and set forced charging if necessary
void batteryCallBack(const scitos_msgs::BatteryState &msg)
{
	/*TODO learn from experience about energy consumption, plan ahead*/
	ROS_DEBUG("Infremen: Battery level %i %i",msg.lifePercent,minimalBatteryLevel);
	if (!forceCharging){
		if (minimalBatteryLevel > msg.lifePercent) forceCharging = true;
	}
	else{
		if (batteryUndockLevel < msg.lifePercent) forceCharging = false;
	}
}	

/*get robot pose*/
void poseCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
	lastPose = *msg;
	//if (debug) ROS_INFO("Infremen: Robot at %lf %lf %lf.",lastPose.position.x,lastPose.position.y,lastPose.position.z);
}

/*detailed info on interation*/
void interacted(const std_msgs::Int32::ConstPtr& msg)
{
	char testTime[1000];
	ros::Time currentTime = ros::Time::now();
	lastInteractionTime = currentTime.sec;

	time_t timeInfo = currentTime.sec;
{'fraction_non_alphanumeric': 0.10750695088044486, 'fraction_numerical': 0.01552363299351251, 'mean_word_length': 6.7985546522131886, 'pattern_counts': {'":': 0, '<': 59, '<?xml version=': 0, '>': 59, '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': '2635345', 'n_tokens_mistral': 3059, 'n_tokens_neox': 2916, 'n_words': 631}