Turtlesim pose

edit

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;
public:
ShapeAction(std::string name,double gettime) : 
as_(nh_, name),
action_name_(name)
{
secs1=gettime;

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

~ShapeAction(void)
{
}
void goalCB()
{
turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();
  }

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

// 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
  pub_.publish(command_);

} 
else
{          
  ROS_INFO("%s: Succeeded", action_name_.c_str());
  // set the action state to succeeded                                                                                    
  as_.setSucceeded(result_);
}   


}
protected:
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);
ros::spin();
  return 0;
}