#include#include#include"ros/ros.h"#include#include#includeusing namespace std;#define TIME_STEP 32 //clock#define NMOTORS 2 // number of motors#define MAX_SPEED 2.0 //the max speed of motorsros::NodeHandle *n;static int controllerCount;static vector controllerList;ros::ServiceClient timeStepClient; //clock communicate clientwebots_ros::set_int timeStepSrv; //clock service dataros::ServiceClient set_velocity_client; //velocity settting clientwebots_ros::set_float set_velocity_srv; //velocity setting dataros::ServiceClient set_position_client; //position setting client;webots_ros::set_float set_position_srv; //position setting datadouble speeds[NMOTORS] = {0.0,0.0}; //motors settings value 0--100//config motor name//static const char *motorNames[NMOTORS]={"left_motor","right_motor"};void updatespeed(){for(int i=0;iserviceClient(string("/robot/")+string(motorNames[i])+string("/set_velocity"));set_velocity_srv.request.value = -speeds[i];set_velocity_client.call(set_velocity_srv);}} void controllerNameCallback(const std_msgs::String::ConstPtr &name){controllerCount++;controllerList.push_back(name->data); //add controller into listROS_INFO("controller #%d: %s.",controllerCount,controllerList.back().c_str());}void quit(int sig){ROS_INFO("User stopped the '/robot' node.");timeStepSrv.request.value=0;timeStepClient.call(timeStepSrv);ros::shutdown();exit(0);}void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value){//send control valint send = 0;ROS_INFO("sub keyboard value = %d",value->data);switch(value->data){//turn leftcase 314:speeds[0] = 5.0;speeds[1] = -5.0;send =1;break;//forwardcase 315:speeds[0] = 5.0;speeds[0] = 5.0;send =1;break;//turn rightcase 316:speeds[0] = -5.0;speeds[1] = 5.0;send=1;break;//turn backcase 317:speeds[0] = -5.0;speeds[1] = -5.0;send =1;break;//stopcase 32:speeds[0] = 0;speeds[1] = 0;send =1;break;default:send =0;break;}//updata velocity value when received informationif(send){updatespeed();send=0;}}int main(int argc,char **argv){setlocale(LC_ALL,"");// useed to show chinese charactersstring controllerName;//create a node named robot_init in ros networkros::init(argc,argv,"robot_init",ros::init_options::AnonymousName);n = new ros::NodeHandle;//get exit signalsignal(SIGINT,quit);//subscribed webots all avaliable model_nameros::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 synctimeStepClient = n->serviceClient("robot/robot/time_step");timeStepSrv.request.value = TIME_STEP;//if there are lots of controller in webots ,user need select a controllerif(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 importantnameSub.shutdown();//init motorsfor(int i=0;iserviceClient(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]);elseROS_ERROR("failed to call service set_position on motor %s.",motorNames[i]);//velocity init setting 0;set_velocity_client = n->serviceClient(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]);elseROS_ERROR("failed to call service set_velocity on motor %s.",motorNames[i]);}//service subscribed keyboardros::ServiceClient keyboardEnableClient;webots_ros::set_int keyboardEnablesrv;keyboardEnableClient = n->serviceClient("/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;}