robobuggy / turtlesim_ref / tutorials / draw_square.cpp @ dd5d7f53
History | View | Annotate | Download (3.01 KB)
1 |
#include <boost/bind.hpp> |
---|---|
2 |
#include <ros/ros.h> |
3 |
#include <turtlesim_ref/Pose.h> |
4 |
#include <turtlesim_ref/Velocity.h> |
5 |
#include <std_srvs/Empty.h> |
6 |
|
7 |
turtlesim_ref::PoseConstPtr g_pose; |
8 |
turtlesim_ref::Pose g_goal; |
9 |
|
10 |
enum State
|
11 |
{ |
12 |
FORWARD, |
13 |
STOP_FORWARD, |
14 |
TURN, |
15 |
STOP_TURN, |
16 |
}; |
17 |
|
18 |
State g_state = FORWARD; |
19 |
State g_last_state = FORWARD; |
20 |
bool g_first_goal_set = false; |
21 |
|
22 |
#define PI 3.141592 |
23 |
|
24 |
void poseCallback(const turtlesim_ref::PoseConstPtr& pose) |
25 |
{ |
26 |
g_pose = pose; |
27 |
} |
28 |
|
29 |
bool hasReachedGoal()
|
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 vel_pub, float linear, float angular) |
45 |
{ |
46 |
turtlesim_ref::Velocity vel; |
47 |
vel.linear = linear; |
48 |
vel.angular = angular; |
49 |
vel_pub.publish(vel); |
50 |
} |
51 |
|
52 |
void stopForward(ros::Publisher vel_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(vel_pub, 0, 0); |
66 |
} |
67 |
} |
68 |
|
69 |
void stopTurn(ros::Publisher vel_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(vel_pub, 0, 0); |
83 |
} |
84 |
} |
85 |
|
86 |
|
87 |
void forward(ros::Publisher vel_pub)
|
88 |
{ |
89 |
if (hasReachedGoal())
|
90 |
{ |
91 |
g_state = STOP_FORWARD; |
92 |
commandTurtle(vel_pub, 0, 0); |
93 |
} |
94 |
else
|
95 |
{ |
96 |
commandTurtle(vel_pub, 1.0, 0.0); |
97 |
} |
98 |
} |
99 |
|
100 |
void turn(ros::Publisher vel_pub)
|
101 |
{ |
102 |
if (hasReachedGoal())
|
103 |
{ |
104 |
g_state = STOP_TURN; |
105 |
commandTurtle(vel_pub, 0, 0); |
106 |
} |
107 |
else
|
108 |
{ |
109 |
commandTurtle(vel_pub, 0.0, 0.4); |
110 |
} |
111 |
} |
112 |
|
113 |
void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub) |
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(vel_pub); |
133 |
} |
134 |
else if (g_state == STOP_FORWARD) |
135 |
{ |
136 |
stopForward(vel_pub); |
137 |
} |
138 |
else if (g_state == TURN) |
139 |
{ |
140 |
turn(vel_pub); |
141 |
} |
142 |
else if (g_state == STOP_TURN) |
143 |
{ |
144 |
stopTurn(vel_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 vel_pub = nh.advertise<turtlesim_ref::Velocity>("turtle1/command_velocity", 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, vel_pub)); |
156 |
|
157 |
std_srvs::Empty empty; |
158 |
reset.call(empty); |
159 |
|
160 |
ros::spin(); |
161 |
} |