Code_Q2_Server_11Oct20

#include "ros/ros.h"
#include "geometry_msgs/Point.h"

#include "server_msgs/Swarm.h"

using namespace Crowd;

static CCrowd g_crowd(20, 20, 20, 20, 0, 0); //width, height, completeWidth, completeHeight,widthOffset, heightOffset
static int g_globalDiscomfort = -1;

class CdAtor {
public:
	CdAtor() {
		std::string topicName = "server_msgs/Swarm";
		publisher = n.advertise<geometry_msgs::Point>(topicName, 1000);
	}
	void run(int argc, char* argv[]) {
	     	// Some logic.

		ros::Rate r(5); //5 Hz
		while (ros::ok) {
			display();
			r.sleep();
		}
		ros::shutdown();
	}

	void display() {
		usleep(10000);
		crowdAllocator::Swarm rSwarm;
		for (int swarmCount = 0; swarmCount < g_crowd.SwarmCount();
				++swarmCount) {
			for (int goalCount = 0; goalCount < swarm.GoalCount();
					++goalCount) {

					// some logic
				for (int personCount = 0; personCount < swarm.PersonCount();++personCount) {
					geometry_msgs::Point rPerson; // rPerson = Person instance in ROS format.
					rPerson.x = person.position.x;
					rPerson.y = person.position.y;
					rPerson.z = 0;
					rSwarm.crowds.push_back(rPerson);
				}
			}
		}
		publisher.publish(rSwarm);
	}

private:
	ros::NodeHandle n;
	ros::Publisher publisher;
};

main(int argc, char* argv[]){
	std::string nodeName = "CdAtor";
	ros::init(argc, argv, nodeName);
	CdAtor crowdAlloc = CdAtor();
	crowdAlloc.run(argc, argv);
	return 0;
}