ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Turtlesim pose

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 close merge delete

Sort by » oldest newest most voted

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

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

more