How to wait for a turtlesim pose

edit

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <turtlesim/Velocity.h>
#include <cmath>
#include <math.h>
#include <angles/angles.h>
#include <std_msgs/Float64.h>

class TurtleControl
{
public:

  TurtleControl(std::string name)
  {
    angle_sub = n.subscribe("set_angle", 1, &TurtleControl::setAngleCB, this);
    distance_sub = n.subscribe("set_distance", 1, &TurtleControl::setDistanceCB, this);
    sub = n.subscribe("/turtle1/pose", 1, &TurtleControl::turtlePoseCB, this);
    pub = n.advertise<turtlesim::Velocity>("/turtle1/command_velocity", 1);
    a_scale = 6.0;
    l_scale = 6.0;
    target_angle = 0.0;
    target_distance = 0.0;
    start_angle = 0;
    error_tol = 0.00001;
    start_x = 0;
    start_y = 0;

  }

  ~TurtleControl(void)
  {
      }

  void setAngleCB(const std_msgs::Float64::ConstPtr& msg)
  {
    target_angle = msg->data;

  }

  void setDistanceCB(const std_msgs::Float64::ConstPtr& msg)
  {
    target_distance = msg->data;
    new_distance_command = true;
  }
  void turtlePoseCB(const turtlesim::Pose::ConstPtr& msg)
  {

    if (new_distance_command)
    {
      start_x = msg->x;
      start_y = msg->y;
      new_distance_command= false;
    }

    theta_error = angles::normalize_angle_positive(target_angle - (msg->theta - start_angle));
    distance_error = target_distance - fabs(sqrt((start_x- msg->x)*(start_x-msg->x) + (start_y-msg->y)*(start_y-msg->y)));

    if (distance_error > error_tol)
    {
      command.linear = l_scale*distance_error;
      command.angular = 0;
    }
    else if (distance_error < error_tol && fabs(theta_error)> error_tol)
    {
      command.angular = a_scale*theta_error;
      command.linear = 0;
    }
    else if (distance_error < error_tol && fabs(theta_error)< error_tol)
    {
      command.angular = 0;
      command.linear = 0;
    }
    else
    {
      command.angular = a_scale*theta_error;
      command.linear = l_scale*distance_error;
    }
    // publish the velocity command                                                                                       
    pub.publish(command);

  }
protected:
  ros::NodeHandle n;
  ros::Subscriber sub, angle_sub, distance_sub;
  ros::Publisher pub;
  turtlesim::Velocity command;
  double a_scale ;
  double l_scale ;
  double target_angle;
  double target_distance;
  double start_angle;
  double error_tol;
  double theta_error;
  double distance_error;
  double start_x;
  double start_y;
  bool new_distance_command;
};


int main(int argc, char **argv)
{
  ros::init(argc, argv, "turtle_control");
  TurtleControl turtle(ros::this_node::getName());
  ros::spin();
  return 0;
}

To set the target from the command line

rostopic pub set_angle std_msgs/Float64 3.14

or

rostopic pub set_distance std_msgs/Float64 2