draw_square.cpp
Go to the documentation of this file.
1 #include <boost/bind.hpp>
2 #include <ros/ros.h>
3 #include <turtlesim/Pose.h>
4 #include <geometry_msgs/Twist.h>
5 #include <std_srvs/Empty.h>
6 
7 turtlesim::PoseConstPtr g_pose;
8 turtlesim::Pose g_goal;
9 
10 enum State
11 {
16 };
17 
20 bool g_first_goal_set = false;
21 
22 #define PI 3.141592
23 
24 void poseCallback(const turtlesim::PoseConstPtr& pose)
25 {
26  g_pose = pose;
27 }
28 
30 {
31  return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;
32 }
33 
34 bool hasStopped()
35 {
36  return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;
37 }
38 
39 void printGoal()
40 {
41  ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
42 }
43 
44 void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
45 {
46  geometry_msgs::Twist twist;
47  twist.linear.x = linear;
48  twist.angular.z = angular;
49  twist_pub.publish(twist);
50 }
51 
52 void stopForward(ros::Publisher twist_pub)
53 {
54  if (hasStopped())
55  {
56  ROS_INFO("Reached goal");
57  g_state = TURN;
58  g_goal.x = g_pose->x;
59  g_goal.y = g_pose->y;
60  g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
61  printGoal();
62  }
63  else
64  {
65  commandTurtle(twist_pub, 0, 0);
66  }
67 }
68 
69 void stopTurn(ros::Publisher twist_pub)
70 {
71  if (hasStopped())
72  {
73  ROS_INFO("Reached goal");
74  g_state = FORWARD;
75  g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
76  g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
77  g_goal.theta = g_pose->theta;
78  printGoal();
79  }
80  else
81  {
82  commandTurtle(twist_pub, 0, 0);
83  }
84 }
85 
86 
87 void forward(ros::Publisher twist_pub)
88 {
89  if (hasReachedGoal())
90  {
92  commandTurtle(twist_pub, 0, 0);
93  }
94  else
95  {
96  commandTurtle(twist_pub, 1.0, 0.0);
97  }
98 }
99 
100 void turn(ros::Publisher twist_pub)
101 {
102  if (hasReachedGoal())
103  {
104  g_state = STOP_TURN;
105  commandTurtle(twist_pub, 0, 0);
106  }
107  else
108  {
109  commandTurtle(twist_pub, 0.0, 0.4);
110  }
111 }
112 
114 {
115  if (!g_pose)
116  {
117  return;
118  }
119 
120  if (!g_first_goal_set)
121  {
122  g_first_goal_set = true;
123  g_state = FORWARD;
124  g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
125  g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
126  g_goal.theta = g_pose->theta;
127  printGoal();
128  }
129 
130  if (g_state == FORWARD)
131  {
132  forward(twist_pub);
133  }
134  else if (g_state == STOP_FORWARD)
135  {
136  stopForward(twist_pub);
137  }
138  else if (g_state == TURN)
139  {
140  turn(twist_pub);
141  }
142  else if (g_state == STOP_TURN)
143  {
144  stopTurn(twist_pub);
145  }
146 }
147 
148 int main(int argc, char** argv)
149 {
150  ros::init(argc, argv, "draw_square");
151  ros::NodeHandle nh;
152  ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);
153  ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
154  ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset");
155  ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub));
156 
157  std_srvs::Empty empty;
158  reset.call(empty);
159 
160  ros::spin();
161 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
int main(int argc, char **argv)
bool hasReachedGoal()
Definition: draw_square.cpp:29
void publish(const boost::shared_ptr< M > &message) const
void stopForward(ros::Publisher twist_pub)
Definition: draw_square.cpp:52
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void turn(ros::Publisher twist_pub)
void poseCallback(const turtlesim::PoseConstPtr &pose)
Definition: draw_square.cpp:24
void stopTurn(ros::Publisher twist_pub)
Definition: draw_square.cpp:69
bool g_first_goal_set
Definition: draw_square.cpp:20
#define PI
Definition: draw_square.cpp:22
State
Definition: draw_square.cpp:10
void printGoal()
Definition: draw_square.cpp:39
State g_state
Definition: draw_square.cpp:18
#define ROS_INFO(...)
void forward(ros::Publisher twist_pub)
Definition: draw_square.cpp:87
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
Definition: draw_square.cpp:44
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
turtlesim::PoseConstPtr g_pose
Definition: draw_square.cpp:7
void timerCallback(const ros::TimerEvent &, ros::Publisher twist_pub)
bool hasStopped()
Definition: draw_square.cpp:34
turtlesim::Pose g_goal
Definition: draw_square.cpp:8
State g_last_state
Definition: draw_square.cpp:19


turtlesim
Author(s): Josh Faust
autogenerated on Fri Jun 7 2019 22:01:46