View difference between Paste ID: VtfpM6Yk and CTCJtdvK

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