Revision 08be08ec
Ran astyle, though astyle options still need to be refined
buggysim/tutorials/draw_square.cpp | ||
---|---|---|
7 | 7 |
turtlesim::PoseConstPtr g_pose; |
8 | 8 |
turtlesim::Pose g_goal; |
9 | 9 |
|
10 |
enum State |
|
11 |
{ |
|
12 |
FORWARD, |
|
13 |
STOP_FORWARD, |
|
14 |
TURN, |
|
15 |
STOP_TURN, |
|
10 |
enum State { |
|
11 |
FORWARD, |
|
12 |
STOP_FORWARD, |
|
13 |
TURN, |
|
14 |
STOP_TURN, |
|
16 | 15 |
}; |
17 | 16 |
|
18 | 17 |
State g_state = FORWARD; |
... | ... | |
23 | 22 |
|
24 | 23 |
void poseCallback(const turtlesim::PoseConstPtr& pose) |
25 | 24 |
{ |
26 |
g_pose = pose; |
|
25 |
g_pose = pose;
|
|
27 | 26 |
} |
28 | 27 |
|
29 | 28 |
bool hasReachedGoal() |
30 | 29 |
{ |
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; |
|
30 |
return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 |
|
31 |
&& fabsf(g_pose->theta - g_goal.theta) < 0.01; |
|
32 | 32 |
} |
33 | 33 |
|
34 | 34 |
bool hasStopped() |
35 | 35 |
{ |
36 |
return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001; |
|
36 |
return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;
|
|
37 | 37 |
} |
38 | 38 |
|
39 | 39 |
void printGoal() |
40 | 40 |
{ |
41 |
ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta); |
|
41 |
ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
|
|
42 | 42 |
} |
43 | 43 |
|
44 | 44 |
void commandTurtle(ros::Publisher vel_pub, float linear, float angular) |
45 | 45 |
{ |
46 |
turtlesim::Velocity vel; |
|
47 |
vel.linear = linear; |
|
48 |
vel.angular = angular; |
|
49 |
vel_pub.publish(vel); |
|
46 |
turtlesim::Velocity vel;
|
|
47 |
vel.linear = linear;
|
|
48 |
vel.angular = angular;
|
|
49 |
vel_pub.publish(vel);
|
|
50 | 50 |
} |
51 | 51 |
|
52 | 52 |
void stopForward(ros::Publisher vel_pub) |
53 | 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 |
} |
|
54 |
if (hasStopped()) { |
|
55 |
ROS_INFO("Reached goal"); |
|
56 |
g_state = TURN; |
|
57 |
g_goal.x = g_pose->x; |
|
58 |
g_goal.y = g_pose->y; |
|
59 |
g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI); |
|
60 |
printGoal(); |
|
61 |
} else { |
|
62 |
commandTurtle(vel_pub, 0, 0); |
|
63 |
} |
|
67 | 64 |
} |
68 | 65 |
|
69 | 66 |
void stopTurn(ros::Publisher vel_pub) |
70 | 67 |
{ |
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 |
} |
|
68 |
if (hasStopped()) { |
|
69 |
ROS_INFO("Reached goal"); |
|
70 |
g_state = FORWARD; |
|
71 |
g_goal.x = cos(g_pose->theta) * 2 + g_pose->x; |
|
72 |
g_goal.y = sin(g_pose->theta) * 2 + g_pose->y; |
|
73 |
g_goal.theta = g_pose->theta; |
|
74 |
printGoal(); |
|
75 |
} else { |
|
76 |
commandTurtle(vel_pub, 0, 0); |
|
77 |
} |
|
84 | 78 |
} |
85 | 79 |
|
86 | 80 |
|
87 | 81 |
void forward(ros::Publisher vel_pub) |
88 | 82 |
{ |
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 |
} |
|
83 |
if (hasReachedGoal()) { |
|
84 |
g_state = STOP_FORWARD; |
|
85 |
commandTurtle(vel_pub, 0, 0); |
|
86 |
} else { |
|
87 |
commandTurtle(vel_pub, 1.0, 0.0); |
|
88 |
} |
|
98 | 89 |
} |
99 | 90 |
|
100 | 91 |
void turn(ros::Publisher vel_pub) |
101 | 92 |
{ |
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 |
} |
|
93 |
if (hasReachedGoal()) { |
|
94 |
g_state = STOP_TURN; |
|
95 |
commandTurtle(vel_pub, 0, 0); |
|
96 |
} else { |
|
97 |
commandTurtle(vel_pub, 0.0, 0.4); |
|
98 |
} |
|
111 | 99 |
} |
112 | 100 |
|
113 | 101 |
void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub) |
114 | 102 |
{ |
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 |
} |
|
103 |
if (!g_pose) { |
|
104 |
return; |
|
105 |
} |
|
106 |
|
|
107 |
if (!g_first_goal_set) { |
|
108 |
g_first_goal_set = true; |
|
109 |
g_state = FORWARD; |
|
110 |
g_goal.x = cos(g_pose->theta) * 2 + g_pose->x; |
|
111 |
g_goal.y = sin(g_pose->theta) * 2 + g_pose->y; |
|
112 |
g_goal.theta = g_pose->theta; |
|
113 |
printGoal(); |
|
114 |
} |
|
115 |
|
|
116 |
if (g_state == FORWARD) { |
|
117 |
forward(vel_pub); |
|
118 |
} else if (g_state == STOP_FORWARD) { |
|
119 |
stopForward(vel_pub); |
|
120 |
} else if (g_state == TURN) { |
|
121 |
turn(vel_pub); |
|
122 |
} else if (g_state == STOP_TURN) { |
|
123 |
stopTurn(vel_pub); |
|
124 |
} |
|
146 | 125 |
} |
147 | 126 |
|
148 | 127 |
int main(int argc, char** argv) |
149 | 128 |
{ |
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::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(); |
|
129 |
ros::init(argc, argv, "draw_square"); |
|
130 |
ros::NodeHandle nh; |
|
131 |
ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback); |
|
132 |
ros::Publisher vel_pub = |
|
133 |
nh.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1); |
|
134 |
ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset"); |
|
135 |
ros::Timer timer = nh.createTimer(ros::Duration(0.016), |
|
136 |
boost::bind(timerCallback, _1, vel_pub)); |
|
137 |
|
|
138 |
std_srvs::Empty empty; |
|
139 |
reset.call(empty); |
|
140 |
|
|
141 |
ros::spin(); |
|
161 | 142 |
} |
Also available in: Unified diff