Ask Your Question

Turtlesim pose

asked 2013-11-07 20:38:13 -0500

Reza1984 gravatar image

I'm using the following code to generate a time-dependent velocity command for the turtle


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

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

ros::NodeHandle n;

ros::Publisher pub = n.advertise<turtlesim::velocity>("/turtle1/command_velocity", 1000); srand(time(0)); double secs1 =ros::Time::now().toSec();

while(pub.getNumSubscribers() == 0) ros::Duration(0.1).sleep();

ros::Rate r(1);

while(true) {

turtlesim::Velocity msg;

double secs2 =ros::Time::now().toSec();

double secs3=secs2-secs1;




ROS_INFO("Sending random velocity command: linear=%f angular=%f time=%f", msg.linear, msg.angular,secs3);


r.sleep(); } }


I don't know how I can change the code to get the position of the turtle at each time?

In fact, I wanna make a feedback control by using the position of the turtle and a desired position.

I really appreciate if you help me in this case!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2013-11-12 15:12:07 -0500

Reza1984 gravatar image

updated 2013-11-12 15:15:23 -0500

I could manage to solve my problem. The following code can do the trajectory (here is line) tracking.

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <turtlesim/Velocity.h>
#include <actionlib/server/simple_action_server.h>
#include <turtle_actionlib/ShapeAction.h>
#include <cmath>
#include <math.h>
#include <angles/angles.h>

class ShapeAction
double secs1;
ShapeAction(std::string name,double gettime) : 
as_(nh_, name),

//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));
as_.registerPreemptCallback(boost::bind(&ShapeAction::preemptCB, this));

//subscribe to the data topic of interest
sub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);
pub_ = nh_.advertise<turtlesim::Velocity>("/turtle1/command_velocity", 1);

void goalCB()
turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();

void preemptCB()
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
void controlCB(const turtlesim::Pose::ConstPtr& msg)
// make sure that the action hasn't been canceled
if (!as_.isActive())

// scalar values for drive the turtle faster and straighter
double l_scale = 0.9;
double a_scale = 0.9;

double secs2 =ros::Time::now().toSec();

double secs3=secs2-secs1;
ROS_INFO("Time=%f", secs3); 

if (secs3 < 17)   
//desired trajectory 
start_x_ = 5+secs3/4;
    start_y_ = 5+secs3/4;
    start_theta_ = M_PI/4;

  dis_error_ =  fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));

  theta_error_ =  start_theta_ - msg->theta ;

  angle_error_ = atan2(start_y_-msg->y,start_x_-msg->x);

  ROS_INFO("angle_error_=%f, theta_error_=%f", angle_error_, theta_error_);

    command_.linear = (sqrt(2)/4)*cos(theta_error_)+l_scale*(start_x_-msg->x);
    command_.angular = l_scale*(start_y_-msg->y) + a_scale*(theta_error_);

  // publish the velocity command

  ROS_INFO("%s: Succeeded", action_name_.c_str());
  // set the action state to succeeded                                                                                    

ros::NodeHandle nh_;
actionlib::SimpleActionServer<turtle_actionlib::ShapeAction> as_;
std::string action_name_;
double start_x_, start_y_, start_theta_;
double dis_error_, theta_error_, angle_error_;
turtlesim::Velocity command_;
turtle_actionlib::ShapeResult result_;
ros::Subscriber sub_;
ros::Publisher pub_;

int main(int argc, char** argv)
ros::init(argc, argv, "turtle_shape");
ros::NodeHandle n;
double secs1=ros::Time::now().toSec();
ShapeAction shape(ros::this_node::getName(),secs1);
  return 0;
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2013-11-07 20:38:13 -0500

Seen: 2,026 times

Last updated: Nov 12 '13