How to wait for a turtlesim pose

Question

I am trying to achieve a specific turtlesim pose (theta only). I send a velocity command to turtlesim makes it rotate through a large angle. I would like to subscribe to the turtlesim pose message and send a new command with angular=0 to stop the rotation at a specific smaller angle (within a tolerance).

I cannot figure out out to read the pose message and send the angular=0 command before the first velocity command expires. Can anyone tell me how to do this? In general, I seem to have a misunderstanding of how to synchronize publish/subscribe events.

I would appreciate any feedback.
Paul.

---

Originally posted by Paul0nc on ROS Answers with karma: 271 on 2012-01-05

Post score: 1

Answer 1

#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

---

Originally posted by mmwise with karma: 8372 on 2012-01-05

This answer was ACCEPTED on the original site

Post score: 5

---

Original comments

Comment by Paul0nc on 2012-01-09:
I now have this working. Thanks mwise_wg for all your help!

Comment by Paul0nc on 2012-01-07:
Again, thanks. Sorry though, I'm not well versed or in this enough yet to understand what's going on in actionlib. How would I send the code you have above two different targets in succession programmatically (i.e. from main() ) ? I'm not sure how to wait for the first command to end.

Comment by mmwise on 2012-01-06:
this is similar to what I did in the actionlib example package. I have updated the code above to help you.

Comment by Paul0nc on 2012-01-06:
THANKS! This works great. A couple of follow up questions though: 1) Is there a corresponding linear controller (or did you write this just for my application)? and 2) How would I get this to execute more than once (to different angles)? Sorry for the newbie questions.