ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
I could mange to solve my problem. The following code can do the trajectory (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;
}
2 | No.2 Revision |
I could mange to solve my problem. The following code can do the trajectory (line) (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;
}
3 | No.3 Revision |
I could mange 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;
}