shape_server.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <turtlesim/Pose.h>
00003 #include <actionlib/server/simple_action_server.h>
00004 #include <cmath>
00005 #include <math.h>
00006 #include <angles/angles.h>
00007 
00008 #include <geometry_msgs/Twist.h>
00009 #include <turtle_actionlib/ShapeAction.h>
00010 
00011 // This class computes the command_velocities of the turtle to draw regular polygons 
00012 class ShapeAction
00013 {
00014 public:
00015   ShapeAction(std::string name) : 
00016     as_(nh_, name, false),
00017     action_name_(name)
00018   {
00019     //register the goal and feeback callbacks
00020     as_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));
00021     as_.registerPreemptCallback(boost::bind(&ShapeAction::preemptCB, this));
00022 
00023     //subscribe to the data topic of interest
00024     sub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);
00025     pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);
00026 
00027     as_.start();
00028   }
00029 
00030   ~ShapeAction(void)
00031   {
00032   }
00033 
00034   void goalCB()
00035   {
00036     // accept the new goal
00037     turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();
00038     //save the goal as private variables
00039     edges_ = goal.edges;
00040     radius_ = goal.radius;
00041 
00042     // reset helper variables
00043     interior_angle_ = ((edges_-2)*M_PI)/edges_;
00044     apothem_ = radius_*cos(M_PI/edges_);
00045     //compute the side length of the polygon
00046     side_len_ = apothem_ * 2* tan( M_PI/edges_);
00047     //store the result values
00048     result_.apothem = apothem_;
00049     result_.interior_angle = interior_angle_;
00050     edge_progress_ =0;
00051     start_edge_ = true;
00052   }
00053 
00054   void preemptCB()
00055   {
00056     ROS_INFO("%s: Preempted", action_name_.c_str());
00057     // set the action state to preempted
00058     as_.setPreempted();
00059   }
00060 
00061   void controlCB(const turtlesim::Pose::ConstPtr& msg)
00062   {
00063     // make sure that the action hasn't been canceled
00064     if (!as_.isActive())
00065       return;
00066   
00067     if (edge_progress_ < edges_)
00068     {
00069       // scalar values for drive the turtle faster and straighter
00070       double l_scale = 6.0;
00071       double a_scale = 6.0;
00072       double error_tol = 0.00001;
00073 
00074       if (start_edge_)
00075       {
00076         start_x_ = msg->x;
00077         start_y_ = msg->y;
00078         start_theta_ = msg->theta;
00079         start_edge_ = false;
00080       }
00081 
00082       // compute the distance and theta error for the shape
00083       dis_error_ = side_len_ - fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));
00084       theta_error_ = angles::normalize_angle_positive(M_PI - interior_angle_ - (msg->theta - start_theta_));
00085      
00086       if (dis_error_ > error_tol)
00087       {
00088         command_.linear.x = l_scale*dis_error_;
00089         command_.angular.z = 0;
00090       }
00091       else if (dis_error_ < error_tol && fabs(theta_error_)> error_tol)
00092       { 
00093         command_.linear.x = 0;
00094         command_.angular.z = a_scale*theta_error_;
00095       }
00096       else if (dis_error_ < error_tol && fabs(theta_error_)< error_tol)
00097       {
00098         command_.linear.x = 0;
00099         command_.angular.z = 0;
00100         start_edge_ = true;
00101         edge_progress_++;
00102       }  
00103       else
00104       {
00105         command_.linear.x = l_scale*dis_error_;
00106         command_.angular.z = a_scale*theta_error_;
00107       } 
00108       // publish the velocity command
00109       pub_.publish(command_);
00110       
00111     } 
00112     else
00113     {          
00114       ROS_INFO("%s: Succeeded", action_name_.c_str());
00115       // set the action state to succeeded
00116       as_.setSucceeded(result_);
00117     }   
00118   }
00119 
00120 protected:
00121   ros::NodeHandle nh_;
00122   actionlib::SimpleActionServer<turtle_actionlib::ShapeAction> as_;
00123   std::string action_name_;
00124   double radius_, apothem_, interior_angle_, side_len_;
00125   double start_x_, start_y_, start_theta_;
00126   double dis_error_, theta_error_;
00127   int edges_ , edge_progress_;
00128   bool start_edge_;
00129   geometry_msgs::Twist command_;
00130   turtle_actionlib::ShapeResult result_;
00131   ros::Subscriber sub_;
00132   ros::Publisher pub_;
00133 };
00134 
00135 int main(int argc, char** argv)
00136 {
00137   ros::init(argc, argv, "turtle_shape");
00138 
00139   ShapeAction shape(ros::this_node::getName());
00140   ros::spin();
00141 
00142   return 0;
00143 }


turtle_actionlib
Author(s): Melonee Wise
autogenerated on Sat Dec 10 2016 03:28:50