Untitled

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

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

bool setGoalAndStart(double x, double y, double z, double ox, double oy, double oz, double ow)
{
  move_base_msgs::MoveBaseGoal Pose;
  MoveBaseClient ac("move_base", true);
  while (!ac.waitForServer(ros::Duration(5.0)))
  {
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  double goal_pose_x = x;
  double goal_pose_y = y;
  double goal_pose_z = z;

  double goal_orientation_x = ox;
  double goal_orientation_y = oy;
  double goal_orientation_z = oz;
  double goal_orientation_w = ow;

  Pose.target_pose.header.frame_id = "map";
  Pose.target_pose.header.stamp = ros::Time::now();
  Pose.target_pose.pose.position.x = goal_pose_x;
  Pose.target_pose.pose.position.y = goal_pose_y;
  Pose.target_pose.pose.position.z = goal_pose_z;
  Pose.target_pose.pose.orientation.x = goal_orientation_x;
  Pose.target_pose.pose.orientation.y = goal_orientation_y;
  Pose.target_pose.pose.orientation.z = goal_orientation_z;
  Pose.target_pose.pose.orientation.w = goal_orientation_w;
  
  // FIX THIS why does it print only once?
  ROS_INFO("Sending goal");
  ROS_INFO_STREAM("GOAL IS:"
                  << "\n"
                  << "x-> " << x << "\n"
                  << "y-> " << y << "\n"
                  << "z-> " << z << "\n"
                  << "ox-> " << ox << "\n"
                  << "oy-> " << oy << "\n"
                  << "oz-> " << oz << "\n"
                  << "ow-> " << ow << "\n");

  ac.sendGoal(Pose);
  ac.waitForResult(ros::Duration(0, 0));

  if (ac.waitForResult() == true) // Source http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals
  {
    ROS_INFO_STREAM("GOAL REACHED"
                    << "\n");
    return true;
  }
  return false;
}

int main(int argc, char **argv)
{

  ros::init(argc, argv, "talker");

  setGoalAndStart(1, 0, 0.0, 0.0, 0.0, 0.0, 1.0);

  setGoalAndStart(1, 1, 0.0, 0.0, 0.0, 0.0, 1.0);

  setGoalAndStart(-2, 1, 0.0, 0.0, 0.0, 0.0, 1.0);

  setGoalAndStart(-4, 2, 0.0, 0.0, 0.0, 0.0, 1.0);

  return 0;
}