Ask Your Question
0

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;

msg.linear=.2*secs3;

msg.angular=.2*secs3;

pub.publish(msg);

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

ros::spinOnce();

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
0

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

Stats

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

Seen: 2,032 times

Last updated: Nov 12 '13