Send goal to robot
----------------
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <trsimcpp/threadsafe_stack_wrapper.h>
#include <future>         // std::async, std::future
// Initialize stack with -1 error value
threadsafe_stack<int> mystack(-1,1);

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

void sendgoal(std::string robot_name)
{
    //tell the action client that we want to spin a thread by default

    MoveBaseClient ac("/" +  robot_name + "/move_base", true);

    //wait for the action server to come up
    while(!ac.waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the move_base action server to come up");
    }

    move_base_msgs::MoveBaseGoal goal;

    //we'll send a goal to the robot to move 1 meter forward
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    goal.target_pose.pose.position.x = 3.0;
    goal.target_pose.pose.orientation.w = 1.0;

    ROS_INFO("Sending goal");
    ac.sendGoal(goal);

    ac.waitForResult();

    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("Status:1");
    else
      ROS_INFO("Status:0");

}


int main(int argc, char** argv){
  ros::init(argc, argv, "simple_navigation_goals");
  // Hold an election
  auto f1 = std::async(std::launch::async,pop);
  auto f2 = std::async(std::launch::async,pop);


      if(f1.get() != -1)
      {
           std::cout <<"robot 1 won!\n";
           sendgoal("robot1");
      }
      else
      {
          std::cout<<"robot 2 won!\n";
          sendgoal("robot2");
      }



  return 0;
}


===========

#ifndef THREADSAFE_STACK_H
#define THREADSAFE_STACK_H
#include <stack>
#include <mutex>
#include <memory>
#include <algorithm>
#include <cassert>

/*
This stack can contain at max 1 element, pushed at the time of creation.
One item can be popped, after which EMPTY_STACK error is returned.
*/


template<typename T>
class threadsafe_stack
{
	public:

	threadsafe_stack(T EMPTY_STACK_, T initval):EMPTY_STACK(EMPTY_STACK_)
	{
		std::lock_guard<std::mutex> lock(m);
		data.push(initval);
	}

	threadsafe_stack(const threadsafe_stack &other): EMPTY_STACK(other.EMPTY_STACK)
	{
		// From Williams' book
		std::lock_guard<std::mutex> lock2(other.m);
		assert (other.data.size() == 1);
		data.push(other.data.top());
	}

	threadsafe_stack& operator = (const threadsafe_stack&) = delete;

	T pop()
	{
		std::lock_guard<std::mutex> lock(m);
		T rv = EMPTY_STACK;
		if(!data.empty())
		{
			rv = data.top();
			data.pop();
		}
		return rv;
	}



	private:
	std::stack<T> data;
	mutable std::mutex m;
	const T EMPTY_STACK;
};