shape_server.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <turtlesim/Pose.h>
4 #include <cmath>
5 #include <math.h>
6 #include <angles/angles.h>
7 
8 #include <geometry_msgs/Twist.h>
9 #include <turtle_actionlib/ShapeAction.h>
10 
11 // This class computes the command_velocities of the turtle to draw regular polygons
13 {
14 public:
15  ShapeAction(std::string name) :
16  as_(nh_, name, false),
17  action_name_(name)
18  {
19  //register the goal and feeback callbacks
20  as_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));
22 
23  //subscribe to the data topic of interest
24  sub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);
25  pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);
26 
27  as_.start();
28  }
29 
31  {
32  }
33 
34  void goalCB()
35  {
36  // accept the new goal
37  turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();
38  //save the goal as private variables
39  edges_ = goal.edges;
40  radius_ = goal.radius;
41 
42  // reset helper variables
43  interior_angle_ = ((edges_-2)*M_PI)/edges_;
44  apothem_ = radius_*cos(M_PI/edges_);
45  //compute the side length of the polygon
46  side_len_ = apothem_ * 2* tan( M_PI/edges_);
47  //store the result values
48  result_.apothem = apothem_;
49  result_.interior_angle = interior_angle_;
50  edge_progress_ =0;
51  start_edge_ = true;
52  }
53 
54  void preemptCB()
55  {
56  ROS_INFO("%s: Preempted", action_name_.c_str());
57  // set the action state to preempted
58  as_.setPreempted();
59  }
60 
61  void controlCB(const turtlesim::Pose::ConstPtr& msg)
62  {
63  // make sure that the action hasn't been canceled
64  if (!as_.isActive())
65  return;
66 
67  if (edge_progress_ < edges_)
68  {
69  // scalar values for drive the turtle faster and straighter
70  double l_scale = 6.0;
71  double a_scale = 6.0;
72  double error_tol = 0.00001;
73 
74  if (start_edge_)
75  {
76  start_x_ = msg->x;
77  start_y_ = msg->y;
78  start_theta_ = msg->theta;
79  start_edge_ = false;
80  }
81 
82  // compute the distance and theta error for the shape
83  dis_error_ = side_len_ - fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));
85 
86  if (dis_error_ > error_tol)
87  {
88  command_.linear.x = l_scale*dis_error_;
89  command_.angular.z = 0;
90  }
91  else if (dis_error_ < error_tol && fabs(theta_error_)> error_tol)
92  {
93  command_.linear.x = 0;
94  command_.angular.z = a_scale*theta_error_;
95  }
96  else if (dis_error_ < error_tol && fabs(theta_error_)< error_tol)
97  {
98  command_.linear.x = 0;
99  command_.angular.z = 0;
100  start_edge_ = true;
101  edge_progress_++;
102  }
103  else
104  {
105  command_.linear.x = l_scale*dis_error_;
106  command_.angular.z = a_scale*theta_error_;
107  }
108  // publish the velocity command
110 
111  }
112  else
113  {
114  ROS_INFO("%s: Succeeded", action_name_.c_str());
115  // set the action state to succeeded
117  }
118  }
119 
120 protected:
123  std::string action_name_;
129  geometry_msgs::Twist command_;
130  turtle_actionlib::ShapeResult result_;
133 };
134 
135 int main(int argc, char** argv)
136 {
137  ros::init(argc, argv, "turtle_shape");
138 
140  ros::spin();
141 
142  return 0;
143 }
boost::shared_ptr< const Goal > acceptNewGoal()
double interior_angle_
ros::Publisher pub_
void preemptCB()
void publish(const boost::shared_ptr< M > &message) const
void controlCB(const turtlesim::Pose::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
turtle_actionlib::ShapeResult result_
ros::Subscriber sub_
double side_len_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ShapeAction(std::string name)
ROSCPP_DECL const std::string & getName()
def normalize_angle_positive(angle)
actionlib::SimpleActionServer< turtle_actionlib::ShapeAction > as_
ros::NodeHandle nh_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
#define ROS_INFO(...)
double theta_error_
void registerPreemptCallback(boost::function< void()> cb)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
std::string action_name_
int main(int argc, char **argv)
~ShapeAction(void)
geometry_msgs::Twist command_
double dis_error_
double start_theta_
void registerGoalCallback(boost::function< void()> cb)


turtle_actionlib
Author(s): Melonee Wise
autogenerated on Wed Jun 5 2019 20:54:28