using namespace std;
ros::Publisher marker_pub;
int cwdNum = 0;
int iterated = 0;
void cwdAllocListnerCallback(const cwdnav_common::Swarm::ConstPtr& msg) {
	cwdNum = 0;
	ros::Rate r(30)
	for (int i = 0; i < msg->cwds.size(); i++) {
		cwdnav_common::CwdElement p = msg->cwds[i];
		std::cout << p.header.stamp << " ," << p.position.x << " ," << p.position.y << std::endl; // <--- Print here is compared.
	}
}

void initROS() {
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe(CwdSensorInteraction::TOPICNAME_SWM, 10,
			cwdAllocListnerCallback);
	marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
	ros::spin();
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "CwdSensor");
	initROS();
	return 0;
}