robobuggy / buggyvis / tutorials / draw_square.cpp @ f75a88be
History | View | Annotate | Download (3.1 KB)
1 |
#include <boost/bind.hpp> |
---|---|
2 |
#include <ros/ros.h> |
3 |
#include <buggymsgs/Pose.h> |
4 |
#include <buggymsgs/Velocity.h> |
5 |
#include <std_srvs/Empty.h> |
6 |
|
7 |
turtlesim::PoseConstPtr g_pose; |
8 |
turtlesim::Pose g_goal; |
9 |
|
10 |
enum State {
|
11 |
FORWARD, |
12 |
STOP_FORWARD, |
13 |
TURN, |
14 |
STOP_TURN, |
15 |
}; |
16 |
|
17 |
State g_state = FORWARD; |
18 |
State g_last_state = FORWARD; |
19 |
bool g_first_goal_set = false; |
20 |
|
21 |
#define PI 3.141592 |
22 |
|
23 |
void poseCallback(const buggymsgs::PoseConstPtr& pose) |
24 |
{ |
25 |
g_pose = pose; |
26 |
} |
27 |
|
28 |
bool hasReachedGoal()
|
29 |
{ |
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 |
} |
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::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 |
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 |
} |
64 |
} |
65 |
|
66 |
void stopTurn(ros::Publisher vel_pub)
|
67 |
{ |
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 |
} |
78 |
} |
79 |
|
80 |
|
81 |
void forward(ros::Publisher vel_pub)
|
82 |
{ |
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 |
} |
89 |
} |
90 |
|
91 |
void turn(ros::Publisher vel_pub)
|
92 |
{ |
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 |
} |
99 |
} |
100 |
|
101 |
void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub) |
102 |
{ |
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 |
} |
125 |
} |
126 |
|
127 |
int main(int argc, char** argv) |
128 |
{ |
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(); |
142 |
} |