launch_launcher.cpp

/** @file
 * @brief 全てのロボットの起動停止をこれひとつで管理する
 *  launchは1つのロボットを簡単に操作し, このノードでは全てのロボットを操作する.
 * @author kurenaif
 * @date 2014-10-20
 */
#include <string>
#include <ros/ros.h>
#include <std_msgs/Int64.h>
#include <vector>
#include <sstream>
#include <map>
#include <vrep_common/simRosStopSimulation.h>
#include <memory>


const int FOOT_NAME_SIZE = 4;
const int NEURON_NAME_SIZE = 7;


// /**
//  * @brief ちゃんとロボットが起動したかどうかチェックする.
//  * 
//  *
//  * @param msg Callbackされる変数
//  */
// void LaunchCallback(const std_msgs::Int64::ConstPtr& msg)
// {
// 	g_launchCallback = msg->data;
// }
//
class Launcher
{
	private:
		int m_launchStatus;
		ros::Subscriber m_sub;
		ros::Publisher m_pub;
		std::string m_num;
		ros::ServiceClient m_client_stopSimulation;

	public:
		void callback(const std_msgs::Int64::ConstPtr& msg)
		{
			m_launchStatus = msg->data;
			std::cout << msg->data << std::endl;
		}

		Launcher(ros::NodeHandle *n, std::string num):m_num(num),
		m_sub(n->subscribe("/"+num+"/launch", 100, &Launcher::callback, this)),
		m_pub(n->advertise<std_msgs::Int64>("/"+num+"/launcher", 100)),
		m_launchStatus(-100),
		m_client_stopSimulation(n->serviceClient<vrep_common::simRosStopSimulation>("/"+num+"/vrep/simRosStopSimulation")){}


		int GetLaunchStatus(){return m_launchStatus;}
		void Push(int value)
		{
			std_msgs::Int64 msg;
			msg.data = value;
			m_pub.publish(msg);
		}
		void StopSimulation()
		{
			vrep_common::simRosStopSimulation srv_simRosStopSimulation;
			m_client_stopSimulation.call(srv_simRosStopSimulation);
		}

		~Launcher(){Launcher::StopSimulation();}
};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "launcher");
	ros::NodeHandle n;

	int nON;//nomber Of Node(Nodeの数)

	std::cout << "Hello I'm launcher. I launch CPG. list is show node status. First of all, input the number of node -1 value" << std::endl << ">"; std::cin >> nON;


	std::vector<std::shared_ptr<Launcher> > launcher;

	for(int i=0;i<=nON;++i)
	{
		launcher.push_back(std::make_shared<Launcher>(&n, std::to_string(i)));
	}

	ros::Rate loop_rate(10);

	while(ros::ok())
	{
		bool isAllCallback = true;
		for(int i=0;i<launcher.size();++i)
		{
			isAllCallback = (launcher[i]->GetLaunchStatus() != -100);
			if(!isAllCallback) break;
		}
		if(isAllCallback) break;
		ros::spinOnce();
		loop_rate.sleep();
	}

	while(ros::ok())
	{
		ros::spinOnce();
		static int node=-1;
		static int input=1;

		bool isAllCallback = true;

		if(node == -1)
		{
			for(int i=0;i<launcher.size();++i)
			{
				isAllCallback = (launcher[i]->GetLaunchStatus() == input);
				if(!isAllCallback) break;
			}
		}
		else if(0 <= node && node < launcher.size())
		{
			isAllCallback = (launcher.at(node)->GetLaunchStatus() == input);
		}

		bool isAllEnd = false;
		for(int i=0;i<launcher.size();++i)
		{
			isAllEnd = (launcher[i]->GetLaunchStatus() == 2);
			if(!isAllEnd)
				break;
		}
		if(isAllEnd) break;
		loop_rate.sleep();

		if(isAllCallback)
		{
			std::cout << "|node -1==all -2==state| |state Launch 1, End 0, Quit 2| >"; std::cin >> node >> input;
			if(node == -1) for(int i=0;i<launcher.size();++i) launcher[i]->Push(input);
			else if(node == -2) for(int i=0;i<launcher.size();++i) std::cout << i << ":" << launcher[i]->GetLaunchStatus() << std::endl;
			else if(0 <= node && node < launcher.size()) launcher.at(node)->Push(input);
		}
		else
		{
			if(node == -1)
			{
				for(int i=0;i<launcher.size();++i)
				{
					if(launcher[i]->GetLaunchStatus() != input) launcher[i]->Push(input);
				}
			}
			else if(0 <= node && node < launcher.size()) launcher.at(node)->Push(input);
		}

	}
	return 0;
}