<commit_before>#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"
#include "se306Project/SheepMoveMsg.h"


#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value

enum FSM {FSM_MOVE_FORWARD, FSM_ROTATE};
// Tunable parameters
// TODO: tune parameters as you see fit
const static double MIN_SCAN_ANGLE_RAD = -10.0/180*M_PI;
const static double MAX_SCAN_ANGLE_RAD = +10.0/180*M_PI;
const static float PROXIMITY_RANGE_M = 1; // Should be smaller than sensor_msgs::LaserScan::range_max
const static double FORWARD_SPEED_MPS = 0.2;
const static double ROTATE_SPEED_RADPS = M_PI/2;

class SheepMove {
			
	public:
	float prevclosestRange;
	double linear_x;
	double angular_z;

	//pose of the robot
	double px;
	double py;
	double theta;
	double prevpx;
	double prevpy;
	bool isRotate;

	int checkcount;
	int sheepNum;
	int robotNum;
	std::string moveStatus;
	
	//methods
	SheepMove(int);
	void StageOdom_callback(nav_msgs::Odometry);
	void move(double, double);
	void statusCallback(se306Project::SheepMoveMsg);
	void commandCallback(const sensor_msgs::LaserScan::ConstPtr&);
	void spin();
	void rosSetup(int argc, char **argv);
	
	protected:
	//talkie things
	ros::Publisher commandPub; // Publisher to the simulated robot's velocity command topic
	ros::Publisher sheepPosPub;
	ros::Subscriber laserSub; // Subscriber to the simulated robot's laser scan topic
	ros::Subscriber sheepStatusSub; //Subscriber to sheep_[sheepNum]/move topic
	//move state things
	enum FSM fsm; // Finite state machine for the random walk algorithm
	ros::Time rotateStartTime; // Start time of the rotation
	ros::Time rotateEndTime;
	ros::Duration rotateDuration; // Duration of the rotation
	ros::Subscriber StageOdo_sub;
};
	
void SheepMove::StageOdom_callback(nav_msgs::Odometry msg) {
	//This is the call back function to process odometry messages coming from Stage. 		
		
	px = 5 + msg.pose.pose.position.x;
	py = 10 + msg.pose.pose.position.y;
	//printf("%f",px);
		
	// If the current position is the same the previous position, then the robot is stuck and needs to move around
	if ((px == prevpx) && (py == prevpy)) {
		//msg.pose.pose.position.x = 5;
		//ROS_INFO("Prevpx: %f",prevpx);

		// Note the negative linear_x		
		//linear_x=-0.2;
		//angular_z=1;

		//theta=10;
		//px = 5;
		//py= 5;
		//printf("Robot stuck");
		if (!isRotate) {	
			ROS_INFO("Robot stuck");
			double r2 = (double)rand()/((double)RAND_MAX/(M_PI/2));
			double m2 = (double)rand()/((double)RAND_MAX/0.5);
			//ROS_INFO("r2" << r2);
			move(-m2, r2);
			
		}	
	} else {
		// One the robot becomes unstuck, then it moves around again normally
		//linear_x = 0.2;
		//angular_z = 0.2;
		//ROS_INFO("Robot unstuck");
		//checkcount=0;
	}
	//ROS_INFO("Current x position is: %f", px);
	//ROS_INFO("Current y position is: %f", py);
	prevpx = px;
	prevpy = py;
}


// Send a velocity command
void SheepMove::move(double linearVelMPS, double angularVelRadPS) {
	geometry_msgs::Twist msg; // The default constructor will set all commands to 0
	msg.linear.x = linearVelMPS;
	msg.angular.z = angularVelRadPS;
	commandPub.publish(msg);
}

void SheepMove::statusCallback(se306Project::SheepMoveMsg msg) {
	moveStatus = msg.moveCommand;
	//TODO: set velocity from msg
}


// Process the incoming laser scan message
void SheepMove::commandCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
	if (fsm == FSM_MOVE_FORWARD) {
		// Compute the average range value between MIN_SCAN_ANGLE and MAX_SCAN_ANGLE
		//
		// NOTE: ideally, the following loop should have additional checks to ensure
		// that indices are not out of bounds, by computing:
		//
		//- currAngle = msg->angle_min + msg->angle_increment*currIndex
		//
		// and then ensuring that currAngle <= msg->angle_max
		unsigned int minIndex = ceil((MIN_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		unsigned int maxIndex = ceil((MAX_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		float closestRange = msg->ranges[minIndex];
		
		for (unsigned int currIndex = minIndex + 1; currIndex < maxIndex; currIndex++) {
			if (msg->ranges[currIndex] < closestRange) {
				closestRange = msg->ranges[currIndex];
			}
		}
		//if (closestRange == prevclosestRange) {
		//	ROS_INFO_STREAM("STUCK");
		//	move(-FORWARD_SPEED_MPS, ROTATE_SPEED_RADPS);
		//	//move(0, ROTATE_SPEED_RADPS);
		//} else {
			
			//ROS_INFO_STREAM("Range: " << closestRange);
			prevclosestRange = closestRange;

			if (closestRange < PROXIMITY_RANGE_M) {
				fsm=FSM_ROTATE;
				rotateStartTime=ros::Time::now();
				//TODO: wtf is r2 for?
				float r2 = (float)rand()/((float)RAND_MAX/100);
				rotateDuration=ros::Duration(rand() % 2);
				//fsm= FSM_MOVE_FORWARD;
			}
		//}
	}
}

// Main FSM loop for ensuring that ROS messages are
// processed in a timely manner, and also for sending
// velocity controls to the simulated robot based on the FSM state
void SheepMove::spin() {
	ros::Rate rate(10); // Specify the FSM loop rate in Hz
	while (ros::ok()) { // Keep spinning loop until user presses Ctrl+C
		if(moveStatus.compare("GO") == 0) {
			std_msgs::String msg;
			std::stringstream ss;
			ss << "sheep_" << sheepNum << " -- px:" << px << " py:" << py << " theta:" << theta << " isRotate:" << isRotate;
   			msg.data = ss.str();
   			
   			//ROS_INFO("%s", msg.data.c_str());
   			
			if (fsm == FSM_MOVE_FORWARD) {
				//ROS_INFO_STREAM("Start forward");
				move(FORWARD_SPEED_MPS, 0);
				checkcount++;
				if (checkcount > 3) {
					isRotate=false;
				}
			}
			if (fsm == FSM_ROTATE) {
				//ROS_INFO_STREAM("Start rotate");
				move(0, ROTATE_SPEED_RADPS);
				rotateEndTime=ros::Time::now();
				isRotate=true;
				//ROS_INFO_STREAM("Time: " << rotateEndTime);
			
				if ((rotateEndTime - rotateStartTime) > rotateDuration) {
					fsm=FSM_MOVE_FORWARD;
					//ROS_INFO_STREAM("End rotate");
					checkcount=0;
				}
			}	
		
		sheepPosPub.publish(msg);
		}//if not move, do anything?
	
		ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
		rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
	}
}
	
SheepMove::SheepMove(int number) {
	// Construst a new SheepMove object and hook up this ROS node
	// to the simulated robot's velocity control and laser topics
	//sheepNum = number;
	
	// Initialize random time generator
	srand(time(NULL));
	moveStatus = "GO";
	prevclosestRange = 0;
	checkcount = 0;
	prevpx = 0;
	prevpx = 0;
	sheepNum = number;
	robotNum = 0;
}	

void SheepMove::rosSetup(int argc, char **argv) {
	std::ostringstream convertS;
	std::ostringstream convertR;
	std::string out;
		
	ros::init(argc, argv, "sheepMove", ros::init_options::AnonymousName); // Initiate new ROS node named "sheepMoveX"
	ros::NodeHandle nh;
	ros::NodeHandle n("~");
	if(n.getParam("sheepNum", sheepNum))
	{
		ROS_INFO_STREAM("Got sheepNum");
	}else {
		ROS_INFO_STREAM("Dafuq, ROS?");
	}
	if(n.getParam("robotNum", robotNum))
	{

		ROS_INFO_STREAM("Got robotNum");
	}else {
		ROS_INFO_STREAM("Dafuq, ROS?");
	}
	
	
	fsm = FSM_MOVE_FORWARD;
	
	rotateStartTime = ros::Time(ros::Time::now()); 
	rotateDuration = ros::Duration(0.f);
	
	//setup talkies
	
	// Advertise a new publisher for the simulated robot's velocity command topic
	// (the second argument indicates that if multiple command messages are in<commit_msg>Sheep basic goal movement implemented<commit_after>#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"
#include "se306Project/SheepMoveMsg.h"
#include <angles/angles.h>


#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value

enum FSM {FSM_MOVE_FORWARD, FSM_ROTATE, FSM_GOTO_GOAL};
// Tunable parameters
// TODO: tune parameters as you see fit
const static double MIN_SCAN_ANGLE_RAD = -10.0/180*M_PI;
const static double MAX_SCAN_ANGLE_RAD = +10.0/180*M_PI;
const static float PROXIMITY_RANGE_M = .9; // Should be smaller than sensor_msgs::LaserScan::range_max
const static double FORWARD_SPEED_MPS = 0.2;
const static double ROTATE_SPEED_RADPS = M_PI/2;

class SheepMove {
			
	public:
	float prevclosestRange;
	double linear_x;
	double angular_z;

	//pose of the robot
	double px;
	double py;
	double theta;
	double prevpx;
	double prevpy;
	double goalpx;
	double goalpy;
	bool isRotate;
	
	bool isGoal;
	bool correctHeading;
	
	double currentHeading;	
	double goalHeading;
	double dx;
	double dy;
	
	int checkcount;
	int sheepNum;
	int robotNum;
	std::string moveStatus;
	
	//methods
	SheepMove(int);
	void StageOdom_callback(nav_msgs::Odometry);
	void move(double, double);
	void statusCallback(se306Project::SheepMoveMsg);
	void commandCallback(const sensor_msgs::LaserScan::ConstPtr&);
	void spin();
	void rosSetup(int argc, char **argv);
	
	protected:
	//talkie things
	ros::Publisher commandPub; // Publisher to the simulated robot's velocity command topic
	ros::Publisher sheepPosPub;
	ros::Subscriber laserSub; // Subscriber to the simulated robot's laser scan topic
	ros::Subscriber sheepStatusSub; //Subscriber to sheep_[sheepNum]/move topic
	//move state things
	enum FSM fsm; // Finite state machine for the random walk algorithm
	ros::Time rotateStartTime; // Start time of the rotation
	ros::Time rotateEndTime;
	ros::Duration rotateDuration; // Duration of the rotation
	ros::Subscriber StageOdo_sub;
};
	
void SheepMove::StageOdom_callback(nav_msgs::Odometry msg) {
	//This is the call back function to process odometry messages coming from Stage. 		
		
	px = msg.pose.pose.position.x;
	//px = 5 + msg.pose.pose.position.x;
	//py = 10 + msg.pose.pose.position.y;
	py = msg.pose.pose.position.y;
	//printf("%f",px);
	
	if (!isGoal) {
		
		// If the current position is the same the previous position, then the robot is stuck and needs to move around
		if ((px == prevpx) && (py == prevpy)) {
			//msg.pose.pose.position.x = 5;
			//ROS_INFO("Prevpx: %f",prevpx);

			// Note the negative linear_x		
			//linear_x=-0.2;
			//angular_z=1;

			//theta=10;
			//px = 5;
			//py= 5;
			//printf("Robot stuck");
			if (!isRotate) {	
				ROS_INFO("Robot stuck");
				double r2 = (double)rand()/((double)RAND_MAX/(M_PI/2));
				double m2 = (double)rand()/((double)RAND_MAX/0.5);
				//ROS_INFO("r2" << r2);
				move(-m2, r2);
				
			}	
		} else {
			// One the robot becomes unstuck, then it moves around again normally
			//linear_x = 0.2;
			//angular_z = 0.2;
			//ROS_INFO("Robot unstuck");
			//checkcount=0;
		}
	} else {
		// Goal positions, need to do something like making this a new method that gets passed in the goal positions for where the grass is 
		goalpx=3;
		goalpy=2;
		
		px = msg.pose.pose.position.x;
		py = msg.pose.pose.position.y;
		
		// This is to currently stop them from wandering around.
		//fsm = FSM_GOTO_GOAL;
		
		px= floorf(px * 10 + 0.5) / 10;
		py= floorf(py * 10 + 0.5) / 10;

		currentHeading = angles::normalize_angle_positive(asin(msg.pose.pose.orientation.z) * 2); // Between (0,2Pi)
			
			//if ((goalpx != round(px)) && (goalpy != round(py))) {
				
			if ((fabs(goalpx-px) > 0.1 ) && (fabs(goalpy-py) > 0.1 )) { // Slightly more accurate when going to goal position
			
				if (!correctHeading) {
					
					// Distance between current position and goal position
					dx = fabs(goalpx - px);
					dy = fabs(goalpy - py);
					
					// Calculates angle based on right-hand triangle trigonometry rules
					goalHeading = atan(dy/dx);
					
					//ROS_INFO("goalHeading: %f", goalHeading);
					//ROS_INFO("currentHeading: %f", currentHeading);
					
					// Below checks are to calculate what the final goalHeading is with respect to the whole world
					if ((goalpx > px) && (goalpy > py)) {
						// Don't need to actually do anything to goalHeading here
						
						//ROS_INFO("goalpx > px & goalpy > py");
						
					}
					if ((goalpx < px) && (goalpy < py)) {
						goalHeading= (M_PI + goalHeading);
						//ROS_INFO("goalpx < px & goalpy < py");

					}
					if ((goalpx < px) && (goalpy > py)) {
						goalHeading= M_PI - goalHeading;
						//ROS_INFO("goalpx < px & goalpy > py");

					}
					if ((goalpx > px) && (goalpy < py)) {
						goalHeading= (2*M_PI) - goalHeading;
						//ROS_INFO("goalpx > px & goalpy < py");

					}
					
					// Rounds to 1dp, otherwise it would be near impossible for both headings to be the same
					goalHeading= floorf(goalHeading * 10 + 0.5) / 10; // To round to 2dp, change the 10's to 100
					currentHeading= floorf(currentHeading * 10 + 0.5) / 10;
					ROS_INFO("currentHeading rounded: %f", currentHeading);
					ROS_INFO("goalHeading rounded: %f", goalHeading);
					
					if (currentHeading != goalHeading) {
						
						move(0,1);
					
					} else {	
						
						ROS_INFO("Correct heading");
						
						correctHeading=true;
					}
				} else {
					move(.2,0);
				}
			} else {
					ROS_INFO("Reached goal");
					isGoal=false;
					fsm= FSM_MOVE_FORWARD;
					// If the sheep is dragged to another position, the whole goal process is recalculated
					correctHeading=false;

				}
	}
	//ROS_INFO("Current x position is: %f", px);
	//ROS_INFO("Current y position is: %f", py);
	prevpx = px;
	prevpy = py;
}


// Send a velocity command
void SheepMove::move(double linearVelMPS, double angularVelRadPS) {
	geometry_msgs::Twist msg; // The default constructor will set all commands to 0
	msg.linear.x = linearVelMPS;
	msg.angular.z = angularVelRadPS;
	commandPub.publish(msg);
}

void SheepMove::statusCallback(se306Project::SheepMoveMsg msg) {
	moveStatus = msg.moveCommand;
	//TODO: set velocity from msg
}


// Process the incoming laser scan message
void SheepMove::commandCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
	if (fsm == FSM_MOVE_FORWARD) {
		// Compute the average range value between MIN_SCAN_ANGLE and MAX_SCAN_ANGLE
		//
		// NOTE: ideally, the following loop should have additional checks to ensure
		// that indices are not out of bounds, by computing:
		//
		//- currAngle = msg->angle_min + msg->angle_increment*currIndex
		//
		// and then ensuring that currAngle <= msg->angle_max
		unsigned int minIndex = ceil((MIN_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		unsigned int maxIndex = ceil((MAX_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		float closestRange = msg->ranges[minIndex];
		
		for (unsigned int currIndex = minIndex + 1; currIndex < maxIndex; currIndex++) {
			if (msg->ranges[currIndex] < closestRange) {
				closestRange = msg->ranges[currIndex];
			}
		}
		//if (closestRange == prevclosestRange) {
		//	ROS_INFO_STREAM("STUCK");
		//	move(-FORWARD_SPEED_MPS, ROTATE_SPEED_RADPS);
		//	//move(0, ROTATE_SPEED_RADPS);
		//} else {
			
			//ROS_INFO_STREAM("Range: " << closestRange);
			prevclosestRange = closestRange;

			if (closestRange < PROXIMITY_RANGE_M) {
				fsm=FSM_ROTATE;
				rotateStartTime=ros::Time::now();
				//TODO: wtf is r2 for?
				float r2 = (float)rand()/((float)RAND_MAX/100);
				rotateDuration=ros::Duration(rand() % 2);
				//fsm= FSM_MOVE_FORWARD;
			}
		//}
	}
}

// Main FSM loop for ensuring that ROS messages are
// processed in a timely manner, and also for sending
// velocity controls to the simulated robot based on the FSM state
void SheepMove::spin() {
	ros::Rate rate(10); // Specify the FSM loop rate in Hz
	while (ros::ok()) { // Keep spinning loop until user presses Ctrl+C
		if(moveStatus.compare("GO") == 0) {
			std_msgs::String msg;
			std::stringstream ss;
			ss << "sheep_" << sheepNum << " -- px:" << px << " py:" << py << " theta:" << theta << " isRotate:" << isRotate;
   			msg.data = ss.str();
   			
   			//ROS_INFO("%s", msg.data.c_str());
   			
			if (fsm == FSM_MOVE_FORWARD) {
				//ROS_INFO_STREAM("Start forward");
				move(FORWARD_SPEED_MPS, 0);
				checkcount++;
				if (checkcount > 3) {
					isRotate=false;
					isGoal=false;
				}
			}
			if (fsm == FSM_ROTATE) {
				//ROS_INFO_STREAM("Start rotate");
				move(0, ROTATE_SPEED_RADPS);
				rotateEndTime=ros::Time::now();
				isRotate=true;
				isGoal=false;
				//ROS_INFO_STREAM("Time: " << rotateEndTime);
			
				if ((rotateEndTime - rotateStartTime) > rotateDuration) {
					fsm=FSM_MOVE_FORWARD;
					//ROS_INFO_STREAM("End rotate");
					checkcount=0;
				}
			} 
			if (fsm == FSM_GOTO_GOAL) {
				isGoal=true;
			}	
		
		sheepPosPub.publish(msg);
		}//if not move, do anything? 
	
		ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
		rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
	}
}
	
SheepMove::SheepMove(int number) {
	// Construst a new SheepMove object and hook up this ROS node
	// to the simulated robot's velocity control and laser topics
	//sheepNum = number;
	
	// Initialize random time generator
	srand(time(NULL));
	moveStatus = "GO";
	prevclosestRange = 0;
	checkcount = 0;
	prevpx = 0;
	prevpx = 0;
	sheepNum = number;
	robotNum = 0;
	correctHeading = false;
}	

void SheepMove::rosSetup(int argc, char **argv) {
	std::ostringstream convertS;
	std::ostringstream convertR;
	std::string out;
		
	ros::init(argc, argv, "sheepMove", ros::init_options::AnonymousName); // Initiate new ROS node named "sheepMoveX"
	ros::NodeHandle nh;
	ros::NodeHandle n("~");
	if(n.getParam("sheepNum", sheepNum))
	{
		ROS_INFO_STREAM("Got sheepNum");
	}else {
		ROS_INFO_STREAM("Dafuq, ROS?");
	}
	if(n.getParam("robotNum", robotNum))
	{

		ROS_INFO_STREAM("Got robotNum");
	}else {
		ROS_INFO_STREAM("Dafuq, ROS?");
	}
	
	
	//fsm = FSM_MOVE_FORWARD;
	fsm = FSM_GOTO_GOAL;
	
	rotateStartTime = ros::Time(ros::Time::now()); 
	rotateDuration = ros::Duration(0.f);
	
	//setup talkies
	
	// Advertise a new publisher for the simulated robot's velocity command topic
	// (the second argument indicates that if multiple command messages are in
{'fraction_non_alphanumeric': 0.13260530421216848, 'fraction_numerical': 0.010530421216848674, 'mean_word_length': 3.506402209389907, 'pattern_counts': {'":': 0, '<': 78, '<?xml version=': 0, '>': 44, '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': '2465430', 'n_tokens_mistral': 7312, 'n_tokens_neox': 6271, 'n_words': 1672}