controller robot in webots via keyboard

#include<signal.h>
#include<std_msgs/String.h>
#include"ros/ros.h"


#include<webots_ros/set_float.h>
#include<webots_ros/set_int.h>
#include<webots_ros/Int32Stamped.h>


using namespace std;
#define TIME_STEP 32 //clock
#define NMOTORS 2 // number of motors
#define MAX_SPEED 2.0 //the max speed of motors

ros::NodeHandle *n;

static int controllerCount;
static vector<string> controllerList;

ros::ServiceClient timeStepClient;   //clock communicate client
webots_ros::set_int timeStepSrv;  //clock service data

ros::ServiceClient set_velocity_client;  //velocity settting client
webots_ros::set_float set_velocity_srv; //velocity setting data

ros::ServiceClient set_position_client; //position setting client;
webots_ros::set_float set_position_srv; //position setting data


double speeds[NMOTORS] = {0.0,0.0};    //motors settings value 0--100


//config motor name
//
static const char *motorNames[NMOTORS]={"left_motor","right_motor"};

/***********************************************************
 * function name:updatre speed
 * description ::send velocity data  set_velocity_srv in set_float
 * parameter null
 * return null
 ************************************************************/


void updatespeed(){
	for(int i=0;i<NMOTORS;++i)
	{
		//update velocity
		set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/")+string(motorNames[i])+string("/set_velocity"));
		set_velocity_srv.request.value = -speeds[i];
		set_velocity_client.call(set_velocity_srv);		
			
	}
}


/***************************************************************
 * function name controller name callback
 * description controller callback get current ros robot controller
 * parameter  @name controller name
 * return null
 * ************************************************************/
 
void controllerNameCallback(const std_msgs::String::ConstPtr &name){

	controllerCount++;
	controllerList.push_back(name->data);  //add controller into list
	ROS_INFO("controller #%d: %s.",controllerCount,controllerList.back().c_str());
}


/**************************************************************
 * function name:quit
 * description exit function
 * parameter: @sig signal
 * return null
 * ************************************************************/

void quit(int sig){
	ROS_INFO("User stopped the '/robot' node.");
	timeStepSrv.request.value=0;
	timeStepClient.call(timeStepSrv);
	ros::shutdown();
	exit(0);

}


/************************************************************
 * function name keyboard return function
 * description: when keyboard action enter function
 * parmeter: @value 
 * return    null
 * **********************************************************/

void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value){
	//send control val
	int send = 0;
	ROS_INFO("sub keyboard value = %d",value->data);
	switch(value->data){
		//turn left
		case 314:
			speeds[0] = 5.0;
			speeds[1] = -5.0;
			send =1;
			break;
			//forward
		case 315:
			speeds[0] = 5.0;
			speeds[0] = 5.0;
			send =1;
			break;
			//turn right
		case 316:
			speeds[0] = -5.0;
			speeds[1] = 5.0;
			send=1;
			break;
			//turn  back
		case 317:
			speeds[0] = -5.0;
			speeds[1] = -5.0;
			send =1;
			break;
			//stop
		case 32:
			speeds[0] = 0;
			speeds[1] = 0;
			send =1;
			break;
		default:
			send =0;
			break;
		
	}	

	//updata velocity value when received information
	if(send){
		updatespeed();
		send=0;
	}
}


int main(int argc,char **argv){
	setlocale(LC_ALL,"");// useed to show chinese characters
	string controllerName;


	//create a node named robot_init in ros network
	ros::init(argc,argv,"robot_init",ros::init_options::AnonymousName);
	n = new ros::NodeHandle;
	//get exit signal
	signal(SIGINT,quit);

	//subscribed webots all avaliable model_name
	ros::Subscriber nameSub = n->subscribe("model_name",100,controllerNameCallback);
	while(controllerCount ==0 || controllerCount < nameSub.getNumPublishers()){
		ros::spinOnce();
		ros::spinOnce();
		ros::spinOnce();
	}

	ros::spinOnce();
	//service subscribe time_step & webots keep sync
	timeStepClient = n->serviceClient<webots_ros::set_int>("robot/robot/time_step");
	timeStepSrv.request.value = TIME_STEP;

	//if there are lots of controller in webots ,user need select a controller
	if(controllerCount == 1){
		controllerName = controllerList[0];
	}else{
		int wantedController = 0;
		cout << "Choose the # of the controller you want to use:\n";
		cin >> wantedController;
		if(1 <= wantedController && wantedController <= controllerCount)
			controllerName = controllerList[wantedController -1];
		else {
			ROS_ERROR("invalid number for controller choice");
			return 1;
		}

		
	}
	ROS_INFO("using controller :'%s'",controllerName.c_str());
	//exit theme ,because it is not important
	nameSub.shutdown();

	//init motors
	for(int i=0;i<NMOTORS;++i){
		//position velocity control setting in INFINITY
		set_position_client = n->serviceClient<webots_ros::set_float>(string("/robot/")+string(motorNames[i])+string("/set_position"));
		set_position_srv.request.value = INFINITY;
		if(set_position_client.call(set_position_srv) && set_position_srv.response.success)
			ROS_INFO("position set to INFINITY for motor %s.",motorNames[i]);
		else
			ROS_ERROR("failed to call service set_position on motor %s.",motorNames[i]);

		//velocity init setting 0;
		set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/")+string(motorNames[i])+string("/set_velocity"));
		set_velocity_srv.request.value = 0.0;
		if(set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)
			ROS_INFO("velocity set to 0.0 for motor %s.",motorNames[i]);
		else
			ROS_ERROR("failed to call service set_velocity on motor %s.",motorNames[i]);

	
	}

	//service subscribed keyboard
	ros::ServiceClient keyboardEnableClient;
	webots_ros::set_int keyboardEnablesrv;

	keyboardEnableClient = n->serviceClient<webots_ros::set_int>("/robot/keyboard/enable");
	keyboardEnablesrv.request.value = TIME_STEP;
	if(keyboardEnableClient.call(keyboardEnablesrv)&& keyboardEnablesrv.response.success){
		ros::Subscriber keyboardSub;
		keyboardSub = n->subscribe("/robot/keyboard/key",1,keyboardDataCallback);
		while(keyboardSub.getNumPublishers() == 0){}
		ROS_INFO("Keyboard enabled.");
		ROS_INFO("control directions: ");
		ROS_INFO("    ^ ");
		ROS_INFO("    | ");
		ROS_INFO("<-- | --> ");
		ROS_INFO("    V		");
		ROS_INFO("stop: space");
		ROS_INFO("use the arrows in webost windows to move the robot");
		ROS_INFO("press the end key to stop the node.");
		while(ros::ok()){
			ros::spinOnce();
			if(!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success){
				ROS_ERROR("failed to call service time_step for next step.");
				break;

			}

			ros::spinOnce();
		}
	}else{
		ROS_ERROR("could not enable keyboard, success = %d.",keyboardEnablesrv.response.success);
	}	//exit and clean clock 
		timeStepSrv.request.value= 0;
		timeStepClient.call(timeStepSrv);
		ros::shutdown();
		return 0;
	

}