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");
}
}
