int main(int argc, char **argv) { // (when V-REP launches this executable, V-REP will also provide the argument list) //numero de argumentos que mande (se excluye el fantasma que el manda solo) if (argc>=1) { //str=atoi(argv[1]); //N obstaculos } else { printf("Indique argumentos!\n"); sleep(5000); return 0; } infoMapa1.coordenadasCelda_y = atoi(argv[1])-1; // Reajuste de indice infoMapa1.coordenadasCelda_x = atoi(argv[2])-1; // Reajuste de indice infoMapa2.coordenadasCelda_y = atoi(argv[3])-1; // Reajuste de indice infoMapa2.coordenadasCelda_x = atoi(argv[4])-1; // Reajuste de indice infoMapa1.valorCelda = atoi(argv[5]); infoMapa2.finalCeldas = atoi(argv[6]); usleep(1000); // Create a ROS node. The name has a random component: int _argc = 0; char** _argv = NULL; std::string nodeName("CeldaDeMapa"); std::string Idy(boost::lexical_cast<std::string>(atoi(argv[1]))); std::string Idx(boost::lexical_cast<std::string>(atoi(argv[2]))); Idy+=Idx; nodeName+=Idy; ros::init(_argc,_argv,nodeName.c_str()); //ROS_INFO("Hola Celdas!"); ros::NodeHandle n; ros::Publisher chatter_pub1 = n.advertise<camina::InfoMapa>("InfoDeMapa1", 1000); ros::Publisher chatter_pub2 = n.advertise<camina::InfoMapa>("InfoDeMapa2", 1000); ros::Subscriber subInfo=n.subscribe("/vrep/info",1000,infoCallback); //ros::Rate wait_rate(atoi(argv[1])+atoi(argv[2])); ros::Rate wait_rate(2); wait_rate.sleep(); //MUY NECESARIO... para dar tiempo a mensajes chatter_pub1.publish(infoMapa1); wait_rate.sleep(); //MUY NECESARIO... para dar tiempo a mensajes chatter_pub2.publish(infoMapa2); //ROS_INFO("Adios1!"); ros::shutdown(); return 0; }
void goToLocation() { /*from go_to_location*/ Client client("action_executor/execute_plan", true); client.waitForServer(); bwi_kr_execution::ExecutePlanGoal goal; bwi_kr_execution::AspRule rule; bwi_kr_execution::AspFluent fluent; fluent.name = "not at"; fluent.variables.push_back(location); rule.body.push_back(fluent); goal.aspGoal.push_back(rule); ROS_INFO("sending goal"); client.sendGoal(goal); ros::Rate wait_rate(10); while(ros::ok() && !client.getState().isDone()) wait_rate.sleep(); if (!client.getState().isDone()) { ROS_INFO("Canceling goal"); client.cancelGoal(); //and wait for canceling confirmation for(int i = 0; !client.getState().isDone() && i < 50; ++i) wait_rate.sleep(); } if (client.getState() == actionlib::SimpleClientGoalState::ABORTED) { ROS_INFO("Aborted"); } else if (client.getState() == actionlib::SimpleClientGoalState::PREEMPTED) { ROS_INFO("Preempted"); } else if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_INFO("Succeeded!"); } else ROS_INFO("Terminated"); }
void FaceDoor::executeAction(const twitcher_actions::FaceDoorGoal::ConstPtr& goal) { twitcher_actions::FaceDoorResult result; // Wait for action executor server to start up Client client("action_executor/execute_plan", true); client.waitForServer(); bwi_kr_execution::ExecutePlanGoal actionGoal; // Matteo-created code that has saved us // Create ASP Rule to determine completion of goal bwi_kr_execution::AspRule rule; bwi_kr_execution::AspFluent fluent; fluent.name = "not facing"; fluent.variables.push_back(goal->door_name); rule.body.push_back(fluent); actionGoal.aspGoal.push_back(rule); ROS_INFO_STREAM("Sending goal: Going to location " << goal->door_name); client.sendGoal(actionGoal); ros::Rate wait_rate(10); while(ros::ok() && !client.getState().isDone()) wait_rate.sleep(); if (!client.getState().isDone()) { ROS_INFO("Canceling goal since it is not done"); client.cancelGoal(); //and wait for canceling confirmation for(int i = 0; !client.getState().isDone() && i<50; ++i) wait_rate.sleep(); } if (client.getState() == actionlib::SimpleClientGoalState::ABORTED) { result.success = false; server->setAborted(result); ROS_INFO("Aborted"); } else if (client.getState() == actionlib::SimpleClientGoalState::PREEMPTED) { result.success = false; server->setPreempted(result); ROS_INFO("Preempted"); } else if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_INFO("Succeeded!"); result.success = true; server->setSucceeded(result); } else { result.success = false; server->setAborted(result); ROS_INFO("Terminated"); } }